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 ea28e16..3267005 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 @@ -7,9 +7,15 @@ 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.MissionFinishAction +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.TelemetrySnapshot +import com.zklh.dronecontroller.core.telemetry.hasReliableRtkPosition +import com.zklh.dronecontroller.core.telemetry.isRtkBlockingWaypointMission import com.zklh.dronecontroller.core.virtualstick.StickPosition import com.zklh.dronecontroller.core.virtualstick.VirtualStickService import dji.sdk.keyvalue.value.flightcontroller.FlyToMode @@ -19,7 +25,14 @@ 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 import kotlinx.coroutines.CoroutineScope import kotlinx.coroutines.Dispatchers import kotlinx.coroutines.Job @@ -31,9 +44,46 @@ 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 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 +private const val PointFlightArriveAltitudeThresholdMeters = 3.0 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 + +interface CloudMissionProgressPublisher { + fun publishFlightTaskProgress( + flightId: String, + status: String, + percent: Int, + waylineId: Int = 0, + currentWaypointIndex: Int = 0, + resultCode: Int = 0, + resultMessage: String = "success" + ) + + fun publishPointFlightProgress( + method: String, + commandId: String, + status: String, + waypointIndex: Int? = null, + remainingDistance: Float? = null, + remainingTime: Float? = null, + resultCode: Int = 0, + resultMessage: String = "success" + ) +} class CloudCommandExecutor( private val flightControl: FlightControlService, @@ -45,7 +95,8 @@ class CloudCommandExecutor( private val cameraControl: CameraControlService = CameraControlService(), private val gimbalControl: GimbalControlService = GimbalControlService(), private val mediaUpload: CloudMediaUploadService? = null, - private val telemetryProvider: () -> TelemetrySnapshot = { TelemetrySnapshot() } + private val telemetryProvider: () -> TelemetrySnapshot = { TelemetrySnapshot() }, + private val progressPublisher: CloudMissionProgressPublisher? = null ) { private val scope = CoroutineScope(SupervisorJob() + Dispatchers.Default) private val virtualStickEnabling = AtomicBoolean(false) @@ -53,6 +104,13 @@ class CloudCommandExecutor( private var lastStickAt = 0L private var lastStickLogAt = 0L private var stickTimeoutJob: Job? = null + private val pointFlightStateLock = Any() + private var startingPointFlightCommandId: String? = null + private var startingPointFlightMissionId: String? = null + private var activePointFlightCommandId: String? = null + private var activePointFlightWaylineMissionId: String? = null + private var lastCompletedPointFlightWaypointIndex: Int? = null + private val pointFlightProgressJobs = ConcurrentHashMap() suspend fun execute(request: CloudCommandRequest): DjiCommandResult { val command = request.normalizedMethod @@ -82,8 +140,8 @@ class CloudCommandExecutor( command == "drc_mode_enter" -> enterDrcMode(data) command == "drc_mode_exit" -> exitDrcMode() command in StickCommands -> executeStickControl(data) - command in TakeoffToPointCommands -> startFlyTo(data) - command in TakeoffCommands && data.hasFlyToTarget() -> startFlyTo(data) + command in TakeoffToPointCommands -> startFlyTo(data, PointFlightProgressKind.TakeoffToPoint) + command in TakeoffCommands && data.hasFlyToTarget() -> startFlyTo(data, PointFlightProgressKind.TakeoffToPoint) command in TakeoffCommands -> startTakeoff(data) command in StopTakeoffCommands -> flightControl.stopTakeoff() command in GoHomeCommands -> flightControl.startGoHome() @@ -101,7 +159,7 @@ class CloudCommandExecutor( command in LaserFillLightCommands -> setLaserFillLight(data) command in LaserMeasureCommands -> setLaserMeasure(data) command in FlyToUpdateCommands -> updateFlyTo(data) - command in FlyToCommands -> startFlyTo(data) + command in FlyToCommands -> startFlyTo(data, PointFlightProgressKind.FlyToPoint) command in StopFlyToCommands -> stopFlyTo() command in WaylinePrepareCommands -> prepareWayline(data) command in WaylineExecuteCommands -> executeWayline(data) @@ -186,10 +244,19 @@ class CloudCommandExecutor( } private suspend fun stopFlyToForManualControl() { - if (!flyToService.isMissionActive()) return - val stopResult = flyToService.stopFlyTo() - if (!stopResult.success) { - Log.w(CloudCommandLogTag, "stop flyTo before manual control failed: ${stopResult.message}") + val pointFlightMissionId = takePointFlightMissionForStop() + if (!flyToService.isMissionActive() && pointFlightMissionId == null) return + if (flyToService.isMissionActive()) { + val stopResult = flyToService.stopFlyTo() + if (!stopResult.success) { + Log.w(CloudCommandLogTag, "stop flyTo before manual control failed: ${stopResult.message}") + } + } + if (pointFlightMissionId != null) { + val stopResult = waypointMission.stopMission(pointFlightMissionId) + if (!stopResult.success) { + Log.w(CloudCommandLogTag, "stop point-flight wayline before manual control failed: ${stopResult.message}") + } } } @@ -200,6 +267,14 @@ class CloudCommandExecutor( "height", "altitude", "alt", + "targetAltitude", + "target_altitude", + "takeoffHeight", + "takeoff_height", + "flyHeight", + "fly_height", + "flightHeight", + "flight_height", "commander_flight_height", "commanderFlightHeight" )?.takeIf { it > MinimumTakeoffTargetHeightMeters } @@ -207,9 +282,26 @@ class CloudCommandExecutor( if (targetHeight == null) { return flightControl.startTakeoff() } - - return DjiCommandResult.failed( - "一键起飞带目标高度时必须同时下发经纬度，请使用 takeoff_to_point/fly_to_point；MSDK 端不再用虚拟杆兜底爬升" + val start = telemetryProvider().pointFlightStartLocation() + ?: return DjiCommandResult.failed("带目标高度的起飞缺少当前位置，无法生成临时航线") + val commandId = data.pointFlightCommandId() + return startFlyToByTemporaryWayline( + command = ParsedFlyToCommand( + commandId = commandId, + waypointIndex = data.pointFlightWaypointIndex() ?: commandId.waypointIndexFromFlyToId(), + rawLatitude = start.latitude, + rawLongitude = start.longitude, + latitude = start.latitude, + longitude = start.longitude, + coordinateType = "wgs84", + requestedHeight = targetHeight, + commanderFlightHeight = targetHeight, + height = targetHeight, + maxSpeed = data.optIntAny(5, "max_speed", "maxSpeed").coerceIn(1, 15), + securityTakeoffHeight = data.optIntAny(20, "security_takeoff_height", "securityTakeoffHeight"), + flyToMode = data.toFlyToMode(defaultMode = FlyToMode.SET_HEIGHT) + ), + flyToFailureMessage = "高度起飞使用临时航线" ) } @@ -287,14 +379,19 @@ class CloudCommandExecutor( return cameraMedia.setLaserMeasureEnabled(data.optBooleanAny("enable", "enabled")) } - private suspend fun startFlyTo(data: JSONObject): DjiCommandResult { - val command = parseFlyToCommand(data) - if (command == null) { + private suspend fun startFlyTo( + data: JSONObject, + progressKind: PointFlightProgressKind = PointFlightProgressKind.FlyToPoint + ): DjiCommandResult { + val parsedCommand = parseFlyToCommand(data) + if (parsedCommand == null) { return DjiCommandResult.failed("指点飞行参数无效：需要 latitude、longitude") } + val command = parsedCommand.withProgressIdentity(progressKind) + existingPointFlightResult(command)?.let { return it } Log.d( CloudCommandLogTag, - "startFlyTo rawLat=${command.rawLatitude} rawLon=${command.rawLongitude} lat=${command.latitude} lon=${command.longitude} " + + "startFlyTo commandId=${command.commandId} rawLat=${command.rawLatitude} rawLon=${command.rawLongitude} lat=${command.latitude} lon=${command.longitude} " + "coordType=${command.coordinateType} height=${command.height} requestedHeight=${command.requestedHeight} " + "maxSpeed=${command.maxSpeed} securityTakeoffHeight=${command.securityTakeoffHeight} flyToMode=${command.flyToMode} data=$data" ) @@ -302,6 +399,15 @@ class CloudCommandExecutor( if (!releaseResult.success) { Log.w(CloudCommandLogTag, "release virtual stick before flyTo failed: ${releaseResult.message}") } + val telemetry = telemetryProvider() + if (telemetry.requiresTemporaryWaylinePointFlight()) { + Log.d( + CloudCommandLogTag, + "M350 detected; bypass native FlyTo and use temporary wayline directly " + + "productType=${telemetry.productType} productTypeValue=${telemetry.productTypeValue}" + ) + return startFlyToByTemporaryWayline(command, "M350 直接使用临时航线执行指点飞行", progressKind) + } val result = flyToService.startFlyTo( latitude = command.latitude, longitude = command.longitude, @@ -310,12 +416,471 @@ class CloudCommandExecutor( securityTakeoffHeight = command.securityTakeoffHeight, flyToMode = command.flyToMode ) - return result + if (result.success) { + clearActivePointFlight() + startPointFlightProgressMonitor(command, progressKind) + return result + } + return if (result.message.isFlyToPointHandlerMissing()) { + startFlyToByTemporaryWayline(command, result.message, progressKind) + } else { + publishPointFlightFailure(command, progressKind, result.message) + result + } + } + + private suspend fun startFlyToByTemporaryWayline( + command: ParsedFlyToCommand, + flyToFailureMessage: String, + progressKind: PointFlightProgressKind = PointFlightProgressKind.FlyToPoint + ): DjiCommandResult { + val telemetry = telemetryProvider() + if (telemetry.isRtkBlockingWaypointMission()) { + publishPointFlightFailure(command, progressKind, "RTK 已开启但尚未就绪") + return DjiCommandResult.failed( + "原生指点飞行不可用；RTK 已开启但尚未就绪（healthy=${telemetry.rtkHealthy}，" + + "solution=${telemetry.rtkPositioningSolution.ifBlank { "UNKNOWN" }}）。" + + "M350 航线任务会被 DJI 拒绝，请等待 RTK 固定解，或在 DJI Pilot 中关闭 RTK 后重试。" + ) + } + val startLocation = telemetry.pointFlightStartLocation() + ?: run { + publishPointFlightFailure(command, progressKind, "当前位置无效，无法生成航线兜底") + return DjiCommandResult.failed( + "原生指点飞行不可用，且当前位置无效，无法生成航线兜底：$flyToFailureMessage" + ) + } + + 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 temporaryWaypoints = temporaryPointFlightWaypoints( + startLocation = startLocation, + command = command, + targetHeight = targetHeight, + speed = speed + ) + val outputFile = File.createTempFile("point-flight-", ".kmz") + val missionId = waypointMission.missionIdFromPath(outputFile.absolutePath) + beginPointFlightStart(command, missionId)?.let { return it } + val plan = MissionPlan( + name = missionId, + waypoints = temporaryWaypoints, + globalSpeed = speed, + takeoffHeight = safeTakeoffHeight, + globalHeight = targetHeight, + finishAction = MissionFinishAction.NoAction + ) + + return runCatching { + stopPreviousPointFlightWayline(command) + WpmlKmzBuilder().generateKmz(plan, outputFile) + Log.d( + CloudCommandLogTag, + "FlyTo handler missing, fallback to temporary wayline commandId=${command.commandId} missionId=$missionId " + + "from=${startLocation.latitude},${startLocation.longitude} " + + "to=${command.latitude},${command.longitude} height=$targetHeight " + + "commanderFlightHeight=${command.commanderFlightHeight} speed=$speed " + + "safeTakeoffHeight=$safeTakeoffHeight waypoints=${temporaryWaypoints.toLogText()} " + + "flyToFailure=$flyToFailureMessage" + ) + val upload = waypointMission.uploadKmzFile(outputFile.absolutePath) + if (!upload.success) { + clearStartingPointFlight(missionId) + publishPointFlightFailure(command, progressKind, upload.message) + return@runCatching DjiCommandResult.failed( + "原生指点飞行不可用，航线兜底上传失败：${upload.message}；原始错误：$flyToFailureMessage" + ) + } + val start = startTemporaryWaylineMission(missionId) + if (start.success) { + markPointFlightActive(command, missionId) + startPointFlightProgressMonitor(command, progressKind, missionId) + DjiCommandResult.ok("原生指点飞行不可用，已切换为临时航线指点飞行：$missionId") + } else { + clearStartingPointFlight(missionId) + publishPointFlightFailure(command, progressKind, start.message) + DjiCommandResult.failed( + "原生指点飞行不可用，航线兜底启动失败：${start.message}；原始错误：$flyToFailureMessage" + ) + } + }.getOrElse { error -> + clearStartingPointFlight(missionId) + publishPointFlightFailure(command, progressKind, error.message ?: error.toString()) + DjiCommandResult.failed("原生指点飞行不可用，生成航线兜底失败：${error.message}；原始错误：$flyToFailureMessage") + } + } + + private fun existingPointFlightResult(command: ParsedFlyToCommand): DjiCommandResult? { + val commandId = command.commandId.takeIf { it.isNotBlank() } ?: return null + return synchronized(pointFlightStateLock) { + when (commandId) { + startingPointFlightCommandId -> DjiCommandResult.ok( + "临时航线指点飞行正在起飞并准备启动：${startingPointFlightMissionId.orEmpty()}" + ) + activePointFlightCommandId -> DjiCommandResult.ok( + "临时航线指点飞行已在执行：${activePointFlightWaylineMissionId.orEmpty()}" + ) + else -> null + } + } + } + + private fun beginPointFlightStart(command: ParsedFlyToCommand, missionId: String): DjiCommandResult? { + val commandId = command.commandId.takeIf { it.isNotBlank() } + return synchronized(pointFlightStateLock) { + when { + commandId != null && commandId == startingPointFlightCommandId -> DjiCommandResult.ok( + "临时航线指点飞行正在起飞并准备启动：${startingPointFlightMissionId.orEmpty()}" + ) + commandId != null && commandId == activePointFlightCommandId -> DjiCommandResult.ok( + "临时航线指点飞行已在执行：${activePointFlightWaylineMissionId.orEmpty()}" + ) + startingPointFlightMissionId != null -> DjiCommandResult.failed( + "上一条临时航线指点飞行正在起飞并准备启动，请稍后重试：${startingPointFlightMissionId.orEmpty()}" + ) + else -> { + startingPointFlightCommandId = commandId + startingPointFlightMissionId = missionId + null + } + } + } + } + + private suspend fun stopPreviousPointFlightWayline(command: ParsedFlyToCommand) { + val commandId = command.commandId.takeIf { it.isNotBlank() } + val missionIdToStop = synchronized(pointFlightStateLock) { + if (commandId != null && activePointFlightCommandId == commandId) { + null + } else { + activePointFlightWaylineMissionId.also { + activePointFlightWaylineMissionId = null + activePointFlightCommandId = null + } + } + } + if (missionIdToStop != null) { + val stopOld = waypointMission.stopMission(missionIdToStop) + if (!stopOld.success) { + Log.w(CloudCommandLogTag, "stop previous point-flight wayline failed: ${stopOld.message}") + } + } + } + + private fun markPointFlightActive(command: ParsedFlyToCommand, missionId: String) { + synchronized(pointFlightStateLock) { + if (startingPointFlightMissionId == missionId) { + startingPointFlightMissionId = null + startingPointFlightCommandId = null + } + activePointFlightWaylineMissionId = missionId + activePointFlightCommandId = command.commandId.takeIf { it.isNotBlank() } + } + } + + private fun clearActivePointFlight() { + synchronized(pointFlightStateLock) { + activePointFlightWaylineMissionId = null + activePointFlightCommandId = null + } + } + + private fun clearActivePointFlightMission(commandId: String, missionId: String) { + synchronized(pointFlightStateLock) { + if (activePointFlightWaylineMissionId == missionId) { + activePointFlightWaylineMissionId = null + } + if (activePointFlightCommandId == commandId) { + activePointFlightCommandId = null + } + } + } + + private fun takePointFlightMissionForStop(): String? { + return synchronized(pointFlightStateLock) { + val missionId = activePointFlightWaylineMissionId ?: startingPointFlightMissionId + activePointFlightWaylineMissionId = null + activePointFlightCommandId = null + startingPointFlightMissionId = null + startingPointFlightCommandId = null + missionId + } + } + + private fun clearStartingPointFlight(missionId: String) { + synchronized(pointFlightStateLock) { + if (startingPointFlightMissionId == missionId) { + startingPointFlightMissionId = null + startingPointFlightCommandId = null + } + } + } + + private fun startPointFlightProgressMonitor( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + temporaryMissionId: String? = null + ) { + val commandId = command.monitorKey(progressKind) + pointFlightProgressJobs.remove(commandId)?.cancel() + publishPointFlightProgress(command, progressKind, "TASK_READY") + pointFlightProgressJobs[commandId] = scope.launch { + val targetLocation = PointFlightStartLocation(command.latitude, command.longitude) + val deadline = System.currentTimeMillis() + PointFlightProgressMonitorTimeoutMs + var lastProgressAt = 0L + var lastRemainingDistance = Double.MAX_VALUE + while (System.currentTimeMillis() < deadline) { + val telemetry = telemetryProvider() + val currentLocation = telemetry.pointFlightStartLocation() + if (currentLocation != null) { + val horizontalDistance = currentLocation.distanceMetersTo(targetLocation) + val altitudeError = abs(command.height - telemetry.altitude) + val remainingDistance = sqrt(horizontalDistance * horizontalDistance + altitudeError * altitudeError) + val remainingSeconds = (remainingDistance / command.maxSpeed.coerceIn(1, 15)).toFloat() + val arrived = horizontalDistance <= PointFlightArriveThresholdMeters && + altitudeError <= PointFlightArriveAltitudeThresholdMeters && + (telemetry.isFlying || telemetry.altitude >= 0.8) + val now = System.currentTimeMillis() + if ( + now - lastProgressAt >= PointFlightProgressIntervalMs || + arrived || + abs(lastRemainingDistance - remainingDistance) >= 5.0 + ) { + publishPointFlightProgress( + command = command, + progressKind = progressKind, + status = "WAYLINE_PROGRESS", + remainingDistance = remainingDistance.toFloat(), + remainingTime = remainingSeconds + ) + lastProgressAt = now + lastRemainingDistance = remainingDistance + } + if (arrived) { + Log.d( + CloudCommandLogTag, + "point flight arrived commandId=$commandId waypointIndex=${command.waypointIndex} " + + "horizontalDistance=${horizontalDistance.formatMeters()} " + + "altitudeError=${altitudeError.formatMeters()} currentAltitude=${telemetry.altitude.formatMeters()} " + + "targetHeight=${command.height.formatMeters()}" + ) + if (!temporaryMissionId.isNullOrBlank()) { + stopTemporaryPointFlightWaylineAfterArrival(commandId, temporaryMissionId) + } + publishPointFlightProgress( + command = command, + progressKind = progressKind, + status = "WAYLINE_OK", + remainingDistance = 0f, + remainingTime = 0f + ) + rememberCompletedPointFlightWaypoint(command) + pointFlightProgressJobs.remove(commandId) + return@launch + } + } + delay(1_000L) + } + pointFlightProgressJobs.remove(commandId) + Log.w( + CloudCommandLogTag, + "point flight progress monitor timed out commandId=$commandId " + + "lat=${command.latitude} lon=${command.longitude} height=${command.height}" + ) + } + } + + private suspend fun stopTemporaryPointFlightWaylineAfterArrival(commandId: String, missionId: String) { + Log.d( + CloudCommandLogTag, + "stop temporary point-flight wayline after arrival commandId=$commandId missionId=$missionId" + ) + val stopResult = waypointMission.stopMission(missionId) + if (stopResult.success) { + Log.d( + CloudCommandLogTag, + "temporary point-flight wayline stopped after arrival commandId=$commandId missionId=$missionId" + ) + } else { + Log.w( + CloudCommandLogTag, + "stop temporary point-flight wayline after arrival failed commandId=$commandId " + + "missionId=$missionId message=${stopResult.message}" + ) + } + clearActivePointFlightMission(commandId, missionId) + delay(TemporaryWaylineExitSettleDelayMs) + } + + private fun publishPointFlightProgress( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + status: String, + remainingDistance: Float? = null, + remainingTime: Float? = null, + resultCode: Int = 0, + resultMessage: String = "success" + ) { + progressPublisher?.publishPointFlightProgress( + method = progressKind.progressMethod, + commandId = command.commandId, + status = status, + waypointIndex = command.waypointIndex, + remainingDistance = remainingDistance, + remainingTime = remainingTime, + resultCode = resultCode, + resultMessage = resultMessage + ) + } + + private fun ParsedFlyToCommand.withProgressIdentity(progressKind: PointFlightProgressKind): ParsedFlyToCommand { + val parsedIndex = waypointIndex ?: commandId.waypointIndexFromFlyToId() + val fallbackIndex = parsedIndex ?: synchronized(pointFlightStateLock) { + when (progressKind) { + PointFlightProgressKind.TakeoffToPoint -> 1 + PointFlightProgressKind.FlyToPoint -> lastCompletedPointFlightWaypointIndex?.plus(1) + } + } + return if (fallbackIndex == waypointIndex) this else copy(waypointIndex = fallbackIndex) + } + + private fun ParsedFlyToCommand.monitorKey(progressKind: PointFlightProgressKind): String = + commandId.takeIf { it.isNotBlank() } + ?: buildString { + append("anonymous-") + append(progressKind.progressMethod) + append("-w") + append(waypointIndex ?: 0) + append('-') + append(latitude) + append('-') + append(longitude) + } + + private fun rememberCompletedPointFlightWaypoint(command: ParsedFlyToCommand) { + val waypointIndex = command.waypointIndex ?: return + synchronized(pointFlightStateLock) { + lastCompletedPointFlightWaypointIndex = waypointIndex + } + } + + private fun publishPointFlightFailure( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + message: String + ) { + publishPointFlightProgress( + command = command, + progressKind = progressKind, + status = "WAYLINE_FAILED", + resultCode = -1, + resultMessage = message + ) + } + + /** + * 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. + */ + 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()} 米" + ) + } + + private suspend fun startTemporaryWaylineMissionWithRetry( + missionId: String, + maxAttempts: Int = WaylineStartRetryCount + ): DjiCommandResult { + var lastResult: DjiCommandResult? = null + for (attempt in 1..maxAttempts) { + val telemetry = telemetryProvider() + Log.d( + CloudCommandLogTag, + "start temporary wayline attempt=$attempt/$maxAttempts missionId=$missionId " + + "isFlying=${telemetry.isFlying} motorsOn=${telemetry.motorsOn} " + + "altitude=${telemetry.altitude} mode=${telemetry.flightMode} " + + "rtkEnabled=${telemetry.rtkEnabled} rtkHealthy=${telemetry.rtkHealthy} " + + "rtkSolution=${telemetry.rtkPositioningSolution}" + ) + val result = waypointMission.startMission(missionId, listOf(0)) + if (result.success) { + return result + } + lastResult = result + Log.w( + CloudCommandLogTag, + "start temporary wayline failed attempt=$attempt/$maxAttempts " + + "missionId=$missionId message=${result.message}" + ) + if (attempt < maxAttempts) { + delay(WaylineStartRetryDelayMs) + } + } + return DjiCommandResult.failed( + "临时航线启动失败，已重试 $maxAttempts 次：${lastResult?.message.orEmpty()}" + ) } private suspend fun updateFlyTo(data: JSONObject): DjiCommandResult { - val command = parseFlyToCommand(data) + val command = parseFlyToCommand(data)?.withProgressIdentity(PointFlightProgressKind.FlyToPoint) ?: return DjiCommandResult.failed("更新指点飞行参数无效：需要 latitude、longitude") + val telemetry = telemetryProvider() + if (telemetry.requiresTemporaryWaylinePointFlight()) { + Log.d( + CloudCommandLogTag, + "M350 detected; use temporary wayline for flyTo update " + + "commandId=${command.commandId} waypointIndex=${command.waypointIndex}" + ) + return startFlyToByTemporaryWayline(command, "M350 使用临时航线执行指点飞行更新", PointFlightProgressKind.FlyToPoint) + } if (!flyToService.isMissionActive()) { Log.d(CloudCommandLogTag, "flyTo update received without active mission, start instead") return startFlyTo(data) @@ -327,14 +892,20 @@ class CloudCommandExecutor( maxSpeed = command.maxSpeed, securityTakeoffHeight = command.securityTakeoffHeight ) - if (!target.success) return target + if (!target.success) { + publishPointFlightFailure(command, PointFlightProgressKind.FlyToPoint, target.message) + return target + } val param = flyToService.updateFlyToParam( height = command.height.roundToInt().coerceAtLeast(1), mode = command.flyToMode ) return if (param.success) { + cancelPointFlightProgressMonitors() + startPointFlightProgressMonitor(command, PointFlightProgressKind.FlyToPoint) DjiCommandResult.ok("${target.message}；${param.message}") } else { + publishPointFlightFailure(command, PointFlightProgressKind.FlyToPoint, "${target.message}；${param.message}") DjiCommandResult.failed("${target.message}；${param.message}") } } @@ -350,9 +921,24 @@ class CloudCommandExecutor( private suspend fun stopFlyTo(): DjiCommandResult { sendNeutralStick() + cancelPointFlightProgressMonitors() + val pointFlightMissionId = takePointFlightMissionForStop() + if (pointFlightMissionId != null) { + val result = waypointMission.stopMission(pointFlightMissionId) + return if (result.success) { + DjiCommandResult.ok("临时航线指点飞行已停止") + } else { + DjiCommandResult.failed("临时航线指点飞行停止失败：${result.message}") + } + } return flyToService.stopFlyTo() } + private fun cancelPointFlightProgressMonitors() { + pointFlightProgressJobs.values.forEach { it.cancel() } + pointFlightProgressJobs.clear() + } + private suspend fun setCameraMode(data: JSONObject): DjiCommandResult { if (data.hasAny("camera_mode", "cameraMode")) { return cameraMedia.setCloudCameraMode(data.optIntAny(-1, "camera_mode", "cameraMode")) @@ -366,15 +952,13 @@ class CloudCommandExecutor( } private suspend fun prepareWayline(data: JSONObject): DjiCommandResult { - val flightId = data.optStringAny("flight_id", "flightId", "job_id", "jobId", "mission_id", "missionId") + val flightId = data.flightTaskId() val kmzPath = data.optStringAny("kmz_path", "kmzPath", "file_path", "filePath", "path") - val fileUrl = data.optStringAny("file_url", "fileUrl", "url") - .ifBlank { - data.optJSONObject("file")?.optStringAny("url", "file_url", "fileUrl").orEmpty() - } + val fileUrl = data.optKmzFileUrl() + val preferredMissionId = data.preferredMissionId(fileUrl) val localKmzPath = when { kmzPath.isNotBlank() -> kmzPath - fileUrl.isNotBlank() -> downloadKmz(fileUrl) + fileUrl.isNotBlank() -> downloadKmz(fileUrl, preferredMissionId) else -> return DjiCommandResult.failed("航线准备失败：未收到 KMZ 本地路径或下载地址") } val upload = waypointMission.uploadKmzFile(localKmzPath) @@ -389,9 +973,31 @@ class CloudCommandExecutor( } private suspend fun executeWayline(data: JSONObject): DjiCommandResult { - val missionId = resolveMissionId(data) - ?: return DjiCommandResult.failed("航线执行失败：缺少 flight_id 或 mission_id") - return waypointMission.startMission(missionId, data.optWaylineIds()) + var missionId = resolveMissionId(data) + if (missionId == null && data.hasKmzFile()) { + val prepare = prepareWayline(data) + if (!prepare.success) return prepare + missionId = resolveMissionId(data) + } + missionId ?: return DjiCommandResult.failed("航线执行失败：缺少 flight_id 或 mission_id") + val result = waypointMission.startMission(missionId, data.optWaylineIds()) + val flightId = data.flightTaskId().ifBlank { missionId } + if (result.success) { + progressPublisher?.publishFlightTaskProgress( + flightId = flightId, + status = "IN_PROGRESS", + percent = 1 + ) + } else { + progressPublisher?.publishFlightTaskProgress( + flightId = flightId, + status = "FAILED", + percent = 0, + resultCode = -1, + resultMessage = result.message + ) + } + return result } private suspend fun stopWayline(data: JSONObject): DjiCommandResult { @@ -406,8 +1012,15 @@ class CloudCommandExecutor( return preparedMissionIds[raw] ?: raw } - private fun downloadKmz(fileUrl: String): String { - val file = File.createTempFile("cloud-wayline-", ".kmz") + private fun downloadKmz(fileUrl: String, preferredMissionId: String): String { + val safeMissionId = preferredMissionId.toSafeMissionFileStem() + val file = if (safeMissionId.isNotBlank()) { + File(cloudWaylineTempDirectory(), "$safeMissionId.kmz").also { + if (it.exists()) it.delete() + } + } else { + File.createTempFile("cloud-wayline-", ".kmz") + } URL(fileUrl).openStream().use { input -> file.outputStream().use { output -> input.copyTo(output) } } @@ -529,21 +1142,145 @@ class CloudCommandExecutor( val yawSpeed = data.optDoubleAny("yaw_speed", "yawSpeed", "gimbal_yaw_speed", "gimbalYawSpeed") return gimbalControl.rotateBySpeed(pitchSpeed, yawSpeed) } + } private data class ParsedFlyToCommand( + val commandId: String, + val waypointIndex: Int?, val rawLatitude: Double, val rawLongitude: Double, val latitude: Double, val longitude: Double, val coordinateType: String, val requestedHeight: Double?, + val commanderFlightHeight: Double?, val height: Double, val maxSpeed: Int, val securityTakeoffHeight: Int, val flyToMode: FlyToMode ) +private enum class PointFlightProgressKind(val progressMethod: String) { + TakeoffToPoint("takeoff_to_point_progress"), + FlyToPoint("fly_to_point_progress") +} + +private data class PointFlightStartLocation( + val latitude: Double, + val longitude: Double +) + +private fun TelemetrySnapshot.pointFlightStartLocation(): PointFlightStartLocation? { + return when { + hasReliableRtkPosition() && isValidCoordinate(rtkLatitude, rtkLongitude) -> + PointFlightStartLocation(rtkLatitude, rtkLongitude) + + locationValid && isValidCoordinate(latitude, longitude) -> + PointFlightStartLocation(latitude, longitude) + + homeLocationValid && isValidCoordinate(homeLatitude, homeLongitude) -> + PointFlightStartLocation(homeLatitude, homeLongitude) + + else -> null + } +} + +private fun TelemetrySnapshot.requiresTemporaryWaylinePointFlight(): Boolean { + val product = productType.uppercase() + return product.contains("MATRICE_350") || + product.contains("M350") +} + +private fun TelemetrySnapshot.isReadyToStartTemporaryWayline(): Boolean = + isFlying || altitude >= 0.8 + +private fun temporaryPointFlightWaypoints( + startLocation: PointFlightStartLocation, + command: ParsedFlyToCommand, + targetHeight: Double, + speed: Double +): 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 departureOffset = if (targetDistance > TemporaryWaylineDepartureOffsetMeters * 2) { + TemporaryWaylineDepartureOffsetMeters + } else { + min(TemporaryWaylineDepartureOffsetMeters, TemporaryWaylineSamePointThresholdMeters + 1.0) + } + val departureLocation = startLocation.offsetBy( + distanceMeters = departureOffset, + bearingRadians = bearing + ) + + return listOf( + MissionWaypoint( + latitude = departureLocation.latitude, + longitude = departureLocation.longitude, + altitude = targetHeight, + speed = speed + ), + MissionWaypoint( + latitude = targetLocation.latitude, + longitude = targetLocation.longitude, + altitude = targetHeight, + speed = speed + ) + ) +} + +private fun PointFlightStartLocation.distanceMetersTo(other: PointFlightStartLocation): Double { + val lat1 = latitude.toRadians() + val lat2 = other.latitude.toRadians() + val dLat = (other.latitude - latitude).toRadians() + val dLon = (other.longitude - longitude).toRadians() + val a = sin(dLat / 2) * sin(dLat / 2) + + cos(lat1) * cos(lat2) * sin(dLon / 2) * sin(dLon / 2) + val normalized = a.coerceIn(0.0, 1.0) + 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}" + } + +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") @@ -578,9 +1315,21 @@ private fun parseFlyToCommand(data: JSONObject): ParsedFlyToCommand? { "height", "altitude", "alt", + "targetAltitude", + "target_altitude", + "takeoffHeight", + "takeoff_height", + "flyHeight", + "fly_height", + "flightHeight", + "flight_height", "commander_flight_height", "commanderFlightHeight" ) + val commanderFlightHeight = data.optDoubleAnyOrNull( + "commander_flight_height", + "commanderFlightHeight" + )?.takeIf { it > 0.0 } val fallbackHeight = data.optDoubleAnyOrNull("security_takeoff_height", "securityTakeoffHeight", "safe_height", "safeHeight") ?.takeIf { it > 0.0 } ?: DefaultFlyToHeightMeters @@ -590,13 +1339,17 @@ private fun parseFlyToCommand(data: JSONObject): ParsedFlyToCommand? { ?: data.optIntAny(15, "max_speed", "maxSpeed") .coerceIn(1, 15) val securityTakeoffHeight = data.optIntAny(20, "security_takeoff_height", "securityTakeoffHeight", "safe_height", "safeHeight") + val commandId = data.pointFlightCommandId() return ParsedFlyToCommand( + commandId = commandId, + waypointIndex = data.pointFlightWaypointIndex() ?: commandId.waypointIndexFromFlyToId(), rawLatitude = rawLatitude, rawLongitude = rawLongitude, latitude = latitude, longitude = longitude, coordinateType = coordinateType, requestedHeight = requestedHeight, + commanderFlightHeight = commanderFlightHeight, height = height, maxSpeed = maxSpeed, securityTakeoffHeight = securityTakeoffHeight, @@ -604,6 +1357,34 @@ private fun parseFlyToCommand(data: JSONObject): ParsedFlyToCommand? { ) } +private fun JSONObject.pointFlightCommandId(): String = + optStringAny( + "flight_id", + "flightId", + "fly_to_id", + "flyToId", + "mission_id", + "missionId", + "job_id", + "jobId", + "id" + ) + +private fun JSONObject.pointFlightWaypointIndex(): Int? = + optIntAny( + Int.MIN_VALUE, + "way_point_index", + "wayPointIndex", + "waypoint_index", + "waypointIndex", + "index" + ).takeIf { it != Int.MIN_VALUE } + +private val FlyToWaypointIndexRegex = Regex("""(?:^|-)w(\d+)$""") + +private fun String.waypointIndexFromFlyToId(): Int? = + FlyToWaypointIndexRegex.find(this)?.groupValues?.getOrNull(1)?.toIntOrNull() + private val PassiveAckCommands = setOf( "flight_authority_grab", "payload_authority_grab", @@ -837,6 +1618,10 @@ private fun JSONObject.hasFlyToTarget(): Boolean { private fun String.shouldLogCommand(): Boolean = this !in PassiveAckCommands && this !in StickCommands +private fun String.isFlyToPointHandlerMissing(): Boolean = + contains("REQUEST_HANDLER_NOT_FOUND", ignoreCase = true) || + contains("FLIGHTCONTROLLER.FlyToPoint", ignoreCase = true) + private fun JSONObject.optWaylineIds(): List { val ids = optJSONArray("wayline_ids") ?: optJSONArray("waylineIds") ?: return listOf(0) return buildList { @@ -845,3 +1630,64 @@ private fun JSONObject.optWaylineIds(): List { } }.ifEmpty { listOf(0) } } + +private fun JSONObject.flightTaskId(): String = + optStringAny("flight_id", "flightId", "job_id", "jobId", "mission_id", "missionId", "id") + +private fun JSONObject.optKmzFileUrl(): String { + val direct = optStringAny( + "file_url", + "fileUrl", + "url", + "kmz_url", + "kmzUrl", + "wayline_url", + "waylineUrl" + ) + if (direct.looksLikeUrl()) return direct + val fileObject = optJSONObject("file") + if (fileObject != null) { + val nested = fileObject.optStringAny("url", "file_url", "fileUrl", "kmz_url", "kmzUrl") + if (nested.looksLikeUrl()) return nested + } + val fileText = optStringAny("file") + return if (fileText.looksLikeUrl()) fileText else "" +} + +private fun JSONObject.hasKmzFile(): Boolean = + optKmzFileUrl().isNotBlank() || + optStringAny("kmz_path", "kmzPath", "file_path", "filePath", "path").isNotBlank() + +private fun JSONObject.preferredMissionId(fileUrl: String): String = + optStringAny("mission_id", "missionId") + .ifBlank { flightTaskId() } + .ifBlank { fileUrl.kmzStemFromUrl() } + +private fun cloudWaylineTempDirectory(): File { + val temp = runCatching { File.createTempFile("cloud-wayline-probe-", ".tmp") } + .getOrNull() + val directory = temp?.parentFile + temp?.delete() + return directory ?: File(".") +} + +private fun String.kmzStemFromUrl(): String { + if (isBlank()) return "" + val path = runCatching { URL(this).path }.getOrDefault(this) + val name = path.substringAfterLast('/').substringBefore('?').substringBefore('#') + return name.removeSuffix(".kmz").removeSuffix(".KMZ") +} + +private fun String.toSafeMissionFileStem(): String = + trim() + .removeSuffix(".kmz") + .removeSuffix(".KMZ") + .replace(Regex("[^A-Za-z0-9_-]"), "_") + .trim('_', '-') + .take(96) + +private fun String.looksLikeUrl(): Boolean { + val value = trim() + return value.startsWith("http://", ignoreCase = true) || + value.startsWith("https://", ignoreCase = true) +} diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudMqttService.kt b/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudMqttService.kt index 6215445..99c36b3 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudMqttService.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudMqttService.kt @@ -4,12 +4,14 @@ import android.util.Log import com.zklh.dronecontroller.core.diagnostics.DroneWarningItem import com.zklh.dronecontroller.core.diagnostics.DroneWarningState import com.zklh.dronecontroller.core.msdk.DroneSdkState +import com.zklh.dronecontroller.core.telemetry.TelemetryCameraIdentity import com.zklh.dronecontroller.core.telemetry.TelemetrySnapshot import dji.v5.manager.diagnostic.WarningLevel import java.util.UUID import java.util.concurrent.atomic.AtomicBoolean import kotlin.math.atan2 import kotlin.math.cos +import kotlin.math.roundToInt import kotlin.math.sin import kotlin.math.sqrt import kotlinx.coroutines.CoroutineScope @@ -35,13 +37,19 @@ import org.json.JSONObject private const val CloudMqttLogTag = "ZklhCloudMqtt" private const val RcDomain = 2 private const val DroneDomain = 0 -private const val RcPlus2Type = 174 -private const val Matrice4Type = 99 private const val RcSubType = 0 -private const val Matrice4SubType = 1 -private const val Matrice4DeviceType = "0-99-1" -private const val Matrice4CameraPayloadIndex = "89-0-0" -private const val Matrice4LiveVideoIndex = "normal-0" +private const val CloudRcType = 56 +private const val CloudRcPlusType = 119 +private const val CloudRcProType = 144 +private const val CloudRcPlus2Type = 174 +private val KnownCloudRcTypes = setOf(CloudRcType, CloudRcPlusType, CloudRcProType, CloudRcPlus2Type) +private const val CloudMatrice4Type = 99 +private const val CloudMatrice4DockType = 100 +private const val CloudMatrice4ThermalPayloadType = 89 +private const val CloudMatrice4DockThermalPayloadType = 99 +private const val MsdkMatrice4SeriesProductType = 150 +private const val MsdkMatrice4ThermalCameraType = 89 +private const val DefaultLiveVideoIndex = "normal-0" private const val ThingVersion = "1.2.0" private const val CloudAccessType = "msdk" private const val OSD_MIN_INTERVAL_MS = 1_000L @@ -61,9 +69,18 @@ private const val OsdCameraClass = "com.dji.sdk.cloudapi.device.OsdCamera" private const val RcDronePayloadClass = "com.dji.sdk.cloudapi.device.RcDronePayload" private const val EarthRadiusMeters = 6_371_000.0 +private data class CloudDeviceDescriptor( + val domain: Int, + val type: Int, + val subType: Int +) { + val deviceType: String + get() = "$domain-$type-$subType" +} + class CloudMqttService( private val loginClient: CloudLoginClient = CloudLoginClient() -) { +) : CloudMissionProgressPublisher { private val scope = CoroutineScope(SupervisorJob() + Dispatchers.IO) private val _state = MutableStateFlow(CloudMqttState()) val state: StateFlow = _state.asStateFlow() @@ -369,24 +386,26 @@ class CloudMqttService( if (rcSn.isBlank()) return val subDevices = JSONArray() if (online && identity.aircraftSn.isNotBlank()) { + val droneDescriptor = telemetry.cloudDroneDescriptor() subDevices.put( JSONObject() .put("sn", identity.aircraftSn) - .put("domain", DroneDomain) - .put("type", Matrice4Type) - .put("sub_type", Matrice4SubType) + .put("domain", droneDescriptor.domain) + .put("type", droneDescriptor.type) + .put("sub_type", droneDescriptor.subType) .put("index", "A") .put("thing_version", ThingVersion) ) } + val rcDescriptor = telemetry.cloudRcDescriptor() val payload = topicRequest(cloudSession) .put("method", "update_topo") .put( "data", JSONObject() - .put("domain", RcDomain) - .put("type", RcPlus2Type) - .put("sub_type", RcSubType) + .put("domain", rcDescriptor.domain) + .put("type", rcDescriptor.type) + .put("sub_type", rcDescriptor.subType) .put("thing_version", ThingVersion) .put("access_type", CloudAccessType) .put("sub_devices", subDevices) @@ -400,23 +419,30 @@ class CloudMqttService( val rcSn = identity.remoteControllerSn val aircraftSn = identity.aircraftSn if (rcSn.isBlank() || aircraftSn.isBlank()) return - val key = "$rcSn/$aircraftSn/${cloudSession.workspaceId}" + val cameraIndexes = telemetry.cloudCameraPayloadIndexes() + val key = "$rcSn/$aircraftSn/${cloudSession.workspaceId}/${cameraIndexes.joinToString(",")}" if (!force && liveCapacityKey == key) return val video = JSONObject() - .put("video_index", Matrice4LiveVideoIndex) + .put("video_index", DefaultLiveVideoIndex) .put("video_type", "normal") .put("switchable_video_types", JSONArray().put("normal")) - val camera = JSONObject() - .put("available_video_number", 1) - .put("coexist_video_number_max", 1) - .put("camera_index", Matrice4CameraPayloadIndex) - .put("video_list", JSONArray().put(video)) + val cameras = JSONArray().apply { + cameraIndexes.forEach { cameraIndex -> + put( + JSONObject() + .put("available_video_number", 1) + .put("coexist_video_number_max", 1) + .put("camera_index", cameraIndex) + .put("video_list", JSONArray().put(JSONObject(video.toString()))) + ) + } + } val device = JSONObject() .put("sn", aircraftSn) - .put("available_video_number", 1) + .put("available_video_number", cameraIndexes.size) .put("coexist_video_number_max", 1) - .put("camera_list", JSONArray().put(camera)) + .put("camera_list", cameras) val payload = topicRequest(cloudSession) .put("gateway", rcSn) .put("method", "livestream_ability_update") @@ -426,7 +452,7 @@ class CloudMqttService( .put( "live_capacity", JSONObject() - .put("available_video_number", 1) + .put("available_video_number", cameraIndexes.size) .put("coexist_video_number_max", 1) .put("device_list", JSONArray().put(device)) ) @@ -453,8 +479,9 @@ class CloudMqttService( } val hmsItems = JSONArray().apply { val inTheSky = telemetry.isFlying || telemetry.motorsOn + val droneDeviceType = telemetry.cloudDroneDescriptor().deviceType warningState.items - .map { it.toHmsJson(inTheSky) } + .map { it.toHmsJson(inTheSky, droneDeviceType) } .forEach { put(it) } } val key = "${identity.remoteControllerSn}/${identity.aircraftSn}/${hmsItems.hmsSignature()}" @@ -480,6 +507,117 @@ class CloudMqttService( } } + override fun publishFlightTaskProgress( + flightId: String, + status: String, + percent: Int, + waylineId: Int, + currentWaypointIndex: Int, + resultCode: Int, + resultMessage: String + ) { + if (flightId.isBlank()) return + val data = JSONObject() + .put("result", cloudApiResult(resultCode, resultMessage)) + .put( + "output", + JSONObject() + .put("status", status) + .put( + "progress", + JSONObject() + .put("percent", percent.coerceIn(0, 100)) + .put("wayline_id", waylineId) + .put("waylineId", waylineId) + .put("current_waypoint_index", currentWaypointIndex) + .put("currentWaypointIndex", currentWaypointIndex) + ) + .put( + "ext", + JSONObject() + .put("flight_id", flightId) + .put("flightId", flightId) + .put("media_count", 0) + .put("mediaCount", 0) + ) + ) + publishCloudEvent("flighttask_progress", flightId, data) + } + + override fun publishPointFlightProgress( + method: String, + commandId: String, + status: String, + waypointIndex: Int?, + remainingDistance: Float?, + remainingTime: Float?, + resultCode: Int, + resultMessage: String + ) { + val isTakeoffToPointProgress = method == "takeoff_to_point_progress" + val effectiveWaypointIndex = waypointIndex ?: if (isTakeoffToPointProgress) 1 else null + if (method.isBlank() || (commandId.isBlank() && effectiveWaypointIndex == null)) return + val data = JSONObject() + .put("status", status.toCloudApiEnumValue()) + .put("result", resultCode) + if (commandId.isNotBlank()) { + if (isTakeoffToPointProgress) { + data.put("flight_id", commandId) + .put("flightId", commandId) + } else { + data.put("fly_to_id", commandId) + .put("flyToId", commandId) + } + } + effectiveWaypointIndex?.let { + data.put("way_point_index", it) + .put("wayPointIndex", it) + .put("waypoint_index", it) + .put("waypointIndex", it) + } + remainingDistance?.let { + data.put("remaining_distance", it) + .put("remainingDistance", it) + } + remainingTime?.let { + val cloudValue: Any = if (isTakeoffToPointProgress) { + it.roundToInt().coerceAtLeast(0) + } else { + it + } + data.put("remaining_time", cloudValue) + .put("remainingTime", cloudValue) + } + publishCloudEvent(method, commandId.ifBlank { "waypoint-$effectiveWaypointIndex" }, data) + } + + private fun publishCloudEvent(method: String, bid: String, data: JSONObject): Boolean { + val cloudSession = session ?: return false + if (!isConnected()) return false + val identity = deviceIdentity + val rcSn = identity.remoteControllerSn + if (rcSn.isBlank()) return false + val fromSn = identity.aircraftSn.ifBlank { rcSn } + val topicSn = fromSn + val payload = topicRequest(cloudSession) + .put("bid", bid.ifBlank { UUID.randomUUID().toString() }) + .put("method", method) + .put("gateway", rcSn) + .put("from", fromSn) + .put("need_reply", false) + .put("data", data) + return publish("thing/product/$topicSn/events", payload, qos = 1) + } + + private fun cloudApiResult(code: Int, message: String): JSONObject = + JSONObject() + .put("code", code) + .put("msg", message) + .put("message", message) + + private fun String.toCloudApiEnumValue(): String = + trim().lowercase() + private fun logHmsDebug(message: String) { if (message == hmsDebugKey) return hmsDebugKey = message @@ -583,9 +721,9 @@ class CloudMqttService( .put("firmware_version", telemetry.firmwareVersion) .put("battery", droneBatteryJson()) .put("position_state", dronePositionStateJson()) - .put("payload", JSONArray().put(rcDronePayloadJson())) + .put("payload", rcDronePayloadListJson()) .put("storage", storageJson()) - .put("cameras", JSONArray().put(osdCameraJson())) + .put("cameras", osdCameraListJson()) .put("height_limit", telemetry.heightLimit) .put("distance_limit_status", rcDistanceLimitStatusJson()) .put("track_id", "") @@ -660,10 +798,17 @@ class CloudMqttService( .put("total", telemetry.storageTotal.toCloudStorageUnit()) .put("used", telemetry.storageUsed().toCloudStorageUnit()) - private fun osdCameraJson(): JSONObject = + private fun osdCameraListJson(): JSONArray = + JSONArray().apply { + telemetry.cloudCameraPayloadIndexes().forEach { payloadIndex -> + put(osdCameraJson(payloadIndex)) + } + } + + private fun osdCameraJson(payloadIndex: String): JSONObject = JSONObject() .put(JsonClassKey, OsdCameraClass) - .put("payload_index", Matrice4CameraPayloadIndex) + .put("payload_index", payloadIndex) .put("camera_mode", telemetry.cameraModeCode()) .put("photo_state", telemetry.photoState) .put("recording_state", telemetry.recordingState) @@ -674,10 +819,17 @@ class CloudMqttService( .put("ir_zoom_factor", telemetry.irZoomFactor) .put("screen_split_enable", false) - private fun rcDronePayloadJson(): JSONObject = + private fun rcDronePayloadListJson(): JSONArray = + JSONArray().apply { + telemetry.cloudCameraPayloadIndexes().forEach { payloadIndex -> + put(rcDronePayloadJson(payloadIndex)) + } + } + + private fun rcDronePayloadJson(payloadIndex: String): JSONObject = JSONObject() .put(JsonClassKey, RcDronePayloadClass) - .put("payload_index", Matrice4CameraPayloadIndex) + .put("payload_index", payloadIndex) .put("gimbal_pitch", telemetry.gimbalPitch.toFloat()) .put("gimbal_roll", telemetry.gimbalRoll.toFloat()) .put("gimbal_yaw", telemetry.gimbalYaw.toFloat()) @@ -824,11 +976,11 @@ class CloudMqttService( } } -private fun DroneWarningItem.toHmsJson(inTheSky: Boolean): JSONObject { +private fun DroneWarningItem.toHmsJson(inTheSky: Boolean, deviceType: String): JSONObject { val hmsCode = code.toHmsCodeOrFallback(message) return JSONObject() .put("code", hmsCode) - .put("device_type", Matrice4DeviceType) + .put("device_type", deviceType) .put("imminent", level.isHmsAlarm()) .put("in_the_sky", inTheSky) .put("level", level.toHmsLevel()) @@ -959,6 +1111,186 @@ private fun TelemetrySnapshot.droneOsdLongitude(): Double = private fun TelemetrySnapshot.droneOsdPositionSource(): String = if (hasUsableRtkPosition()) "RTK" else "FC" +private fun TelemetrySnapshot.cloudRcDescriptor(): CloudDeviceDescriptor = + CloudDeviceDescriptor( + domain = RcDomain, + type = cloudApiRcType(), + subType = RcSubType + ) + +private fun TelemetrySnapshot.cloudApiRcType(): Int { + val rcText = remoteControllerType.uppercase() + return when { + groundDeviceIdentity in KnownCloudRcTypes -> groundDeviceIdentity + remoteControllerTypeValue in KnownCloudRcTypes -> remoteControllerTypeValue + groundDeviceIdentity == 120 || remoteControllerTypeValue == 120 -> CloudRcPlus2Type + rcText.contains("RC_PLUS_2") || rcText.contains("RC PLUS 2") || rcText.contains("RCPLUS2") -> CloudRcPlus2Type + rcText.contains("RC_PLUS") || rcText.contains("RC PLUS") || rcText.contains("RCPLUS") -> CloudRcPlusType + rcText.contains("RC_PRO") || rcText.contains("RC PRO") || rcText.contains("RCPRO") -> CloudRcProType + rcText.contains("RC") -> CloudRcType + else -> CloudRcPlus2Type + } +} + +private fun TelemetrySnapshot.cloudDroneDescriptor(): CloudDeviceDescriptor { + val type = cloudApiDroneType() + return CloudDeviceDescriptor( + domain = DroneDomain, + type = type, + subType = cloudDroneSubType() + ) +} + +private fun TelemetrySnapshot.cloudApiDroneType(): Int { + val product = productType.uppercase() + val camera = cloudCameraText() + return when { + product.contains("MATRICE_350") || product.contains("M350") -> 89 + product.contains("MATRICE_300") || product.contains("M300") -> 60 + product.contains("M30") -> 67 + product.contains("MAVIC_3_ENTERPRISE") -> 77 + product.contains("MATRICE_400") || product.contains("M400") -> 103 + product.contains("MATRICE_4TD") || product.contains("M4TD") || product.contains("4TD") -> CloudMatrice4DockType + product.contains("MATRICE_4D") || product.contains("M4D") || product.contains("4D") -> CloudMatrice4DockType + isMsdkMatrice4SeriesProduct(product) && isMatrice4ThermalCamera(camera) -> CloudMatrice4DockType + product.contains("MATRICE_4T") || product.contains("M4T") || product.contains("4T") -> CloudMatrice4Type + product.contains("MATRICE_4E") || product.contains("M4E") || product.contains("4E") -> CloudMatrice4Type + product.contains("MATRICE_4") -> CloudMatrice4Type + else -> 0 + } +} + +private fun TelemetrySnapshot.cloudDroneSubType(): Int { + val camera = cloudCameraText() + val product = productType.uppercase() + return when { + camera.contains("M4TD") || camera.contains("M4T") -> 1 + camera.contains("M4D") || camera.contains("M4E") -> 0 + camera.contains("M3TD") || camera.contains("M3T") -> 1 + camera.contains("M30T") -> 1 + product.contains("MATRICE_4TD") || product.contains("M4TD") || product.contains("4TD") -> 1 + product.contains("MATRICE_4D") || product.contains("M4D") || product.contains("4D") -> 0 + product.contains("MATRICE_4T") || product.contains("M4T") || product.contains("4T") -> 1 + product.contains("MATRICE_4E") || product.contains("M4E") || product.contains("4E") -> 0 + product.contains("MATRICE_4D") || product.contains("MATRICE_4") -> { + if (camera.contains("M4D") || camera.contains("M4E")) 0 else 1 + } + product.contains("M30_SERIES") || product.contains("MAVIC_3_ENTERPRISE") -> { + if (camera.contains("M30") || camera.contains("M3E") || camera.contains("M3M")) 0 else 1 + } + else -> 0 + } +} + +private fun TelemetrySnapshot.cloudCameraPayloadIndexes(): List { + val detectedIndexes = cameraIdentities + .mapNotNull { it.cloudCameraPayloadIndex(productType) } + .distinct() + return detectedIndexes.ifEmpty { listOf(cloudCameraPayloadIndex()) } +} + +private fun TelemetrySnapshot.cloudCameraPayloadIndex(): String { + val payloadType = cloudPayloadTypeFor( + cameraTypeValue = cameraTypeValue, + cameraText = cloudCameraText(), + productText = productType + ) ?: fallbackCloudPayloadType() + return "${payloadType.type}-${payloadType.subType}-${cameraComponent.toCloudPayloadPositionIndex()}" +} + +private fun TelemetryCameraIdentity.cloudCameraPayloadIndex(productType: String): String? { + val payloadType = cloudPayloadTypeFor( + cameraTypeValue = cameraTypeValue, + cameraText = listOf(cameraType, payloadCameraType).joinToString(" "), + productText = productType + ) ?: return null + return "${payloadType.type}-${payloadType.subType}-${component.toCloudPayloadPositionIndex()}" +} + +private fun TelemetrySnapshot.cloudCameraText(): String = + ( + listOf(cameraType, payloadCameraType) + + cameraIdentities.flatMap { listOf(it.cameraType, it.payloadCameraType) } + ) + .joinToString(" ") + .uppercase() + +private fun TelemetrySnapshot.fallbackCloudPayloadType(): CloudPayloadType = + cloudPayloadTypeFor(cameraTypeValue = 0, cameraText = "", productText = productType) + ?: CloudPayloadType(type = 0, subType = 0) + +private data class CloudPayloadType( + val type: Int, + val subType: Int = 0 +) + +private fun cloudPayloadTypeFor(cameraTypeValue: Int, cameraText: String, productText: String): CloudPayloadType? { + val camera = cameraText.uppercase() + val product = productText.uppercase() + val mapped = when { + camera.contains("P1") || camera.contains("ZENMUSE_P1") -> CloudPayloadType(50, 65535) + camera.contains("H30T") || camera.contains("ZENMUSE_H30T") -> CloudPayloadType(83) + camera.contains("H30") || camera.contains("ZENMUSE_H30") -> CloudPayloadType(82) + camera.contains("H20N") || camera.contains("ZENMUSE_H20N") -> CloudPayloadType(61) + camera.contains("H20T") || camera.contains("ZENMUSE_H20T") -> CloudPayloadType(43) + camera.contains("H20") || camera.contains("ZENMUSE_H20") -> CloudPayloadType(42) + camera.contains("L1") || camera.contains("ZENMUSE_L1") -> CloudPayloadType(53) + camera.contains("L2") || camera.contains("ZENMUSE_L2") -> CloudPayloadType(84) + camera.contains("L3") || camera.contains("ZENMUSE_L3") -> CloudPayloadType(165) + camera.contains("M4TD") -> CloudPayloadType(99) + camera.contains("M4D") -> CloudPayloadType(98) + isMsdkMatrice4SeriesProductText(product) && isMatrice4ThermalCamera(camera) -> CloudPayloadType(CloudMatrice4DockThermalPayloadType) + camera.contains("M4T") -> CloudPayloadType(CloudMatrice4ThermalPayloadType) + camera.contains("M4E") -> CloudPayloadType(88) + camera.contains("M3TD") -> CloudPayloadType(81) + camera.contains("M3D") -> CloudPayloadType(80) + camera.contains("M3TA") -> CloudPayloadType(129) + camera.contains("M3T") -> CloudPayloadType(67) + camera.contains("M3E") -> CloudPayloadType(66) + camera.contains("M30T") -> CloudPayloadType(53) + camera == "M30" || camera.contains(" M30 ") -> CloudPayloadType(52) + product.contains("MATRICE_350") || product.contains("M350") -> CloudPayloadType(43) + product.contains("MATRICE_4TD") || product.contains("M4TD") || product.contains("4TD") -> CloudPayloadType(99) + product.contains("MATRICE_4D") || product.contains("M4D") || product.contains("4D") -> CloudPayloadType(98) + product.contains("MATRICE_4T") || product.contains("M4T") || product.contains("4T") -> CloudPayloadType(89) + product.contains("MATRICE_4E") || product.contains("M4E") || product.contains("4E") -> CloudPayloadType(88) + product.contains("MATRICE_4") -> CloudPayloadType(89) + product.contains("MAVIC_3_ENTERPRISE") -> CloudPayloadType(66) + product.contains("M30_SERIES") -> CloudPayloadType(53) + else -> null + } + if (mapped != null) { + return mapped + } + if (cameraTypeValue > 0) { + return CloudPayloadType( + type = cameraTypeValue, + subType = cameraText.cloudPayloadSubType() + ) + } + return null +} + +private fun TelemetrySnapshot.isMsdkMatrice4SeriesProduct(product: String): Boolean = + productTypeValue == MsdkMatrice4SeriesProductType || isMsdkMatrice4SeriesProductText(product) + +private fun isMsdkMatrice4SeriesProductText(product: String): Boolean = + product.contains("MATRICE_4_SERIES") + +private fun isMatrice4ThermalCamera(camera: String): Boolean = + camera.contains("M4TD") || camera.contains("M4T") || camera.contains(MsdkMatrice4ThermalCameraType.toString()) + +private fun String.cloudPayloadSubType(): Int = + if (uppercase().contains("P1")) 65535 else 0 + +private fun String.toCloudPayloadPositionIndex(): Int = + when (uppercase()) { + "RIGHT", "PORT_2" -> 1 + "UP", "PORT_3" -> 2 + "PORT_4" -> 3 + else -> 0 + } + private fun TelemetrySnapshot.hasFixedPosition(): Boolean = (gpsValid && locationValid) || hasUsableRtkPosition() diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlightControlService.kt b/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlightControlService.kt index cece196..be36c16 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlightControlService.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlightControlService.kt @@ -2,21 +2,41 @@ package com.zklh.dronecontroller.core.flight import com.zklh.dronecontroller.core.msdk.DjiCommandResult import com.zklh.dronecontroller.core.safety.SafetyInterlock +import com.zklh.dronecontroller.core.telemetry.TelemetrySnapshot +import com.zklh.dronecontroller.core.telemetry.isRtkBlockingTakeoff import dji.sdk.keyvalue.key.FlightControllerKey import dji.sdk.keyvalue.value.common.LocationCoordinate3D import dji.sdk.keyvalue.value.flightcontroller.LookAtInfo import dji.sdk.keyvalue.value.flightcontroller.LookAtMode +import dji.v5.common.callback.CommonCallbacks import dji.v5.common.error.IDJIError import dji.v5.et.action import dji.v5.et.create +import dji.v5.manager.aircraft.rtk.RTKCenter import kotlin.coroutines.resume import kotlinx.coroutines.suspendCancellableCoroutine -class FlightControlService { +class FlightControlService( + private val telemetryProvider: () -> TelemetrySnapshot = { TelemetrySnapshot() } +) { suspend fun startTakeoff(): DjiCommandResult = if (!SafetyInterlock.FLIGHT_COMMANDS_ENABLED) { DjiCommandResult.failed(SafetyInterlock.LOCKED_MESSAGE) } else { + val rtkAdjustment = disableUnreadyRtkBeforeTakeoff() + if (rtkAdjustment?.success == false) { + rtkAdjustment + } else { + val takeoffResult = performStartTakeoff() + if (takeoffResult.success && rtkAdjustment != null) { + DjiCommandResult.ok("${rtkAdjustment.message}，${takeoffResult.message}") + } else { + takeoffResult + } + } + } + + private suspend fun performStartTakeoff(): DjiCommandResult = suspendCancellableCoroutine { continuation -> FlightControllerKey.KeyStartTakeoff.create().action({ continuation.resume(DjiCommandResult.ok("自动起飞已开始")) @@ -24,6 +44,36 @@ class FlightControlService { continuation.resume(DjiCommandResult.failed(error)) }) } + + private suspend fun disableUnreadyRtkBeforeTakeoff(): DjiCommandResult? { + val telemetry = telemetryProvider() + if (!telemetry.isRtkBlockingTakeoff()) return null + if (telemetry.motorsOn || telemetry.isFlying) { + return DjiCommandResult.failed("RTK 已开启但尚未就绪，电机已起转，不能自动关闭 RTK") + } + + val disableResult = setAircraftRtkModuleEnabled(false) + return if (disableResult.success) { + DjiCommandResult.ok("RTK 信号未就绪，已先关闭 RTK") + } else { + DjiCommandResult.failed("RTK 信号未就绪，自动关闭 RTK 失败：${disableResult.message}") + } + } + + private suspend fun setAircraftRtkModuleEnabled(enabled: Boolean): DjiCommandResult = + suspendCancellableCoroutine { continuation -> + RTKCenter.getInstance().setAircraftRTKModuleEnabled( + enabled, + object : CommonCallbacks.CompletionCallback { + override fun onSuccess() { + continuation.resume(DjiCommandResult.ok(if (enabled) "RTK 已开启" else "RTK 已关闭")) + } + + override fun onFailure(error: IDJIError) { + continuation.resume(DjiCommandResult.failed(error)) + } + } + ) } suspend fun stopTakeoff(): DjiCommandResult = diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlyToService.kt b/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlyToService.kt index 9b78a4d..2fbd0a8 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlyToService.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/flight/FlyToService.kt @@ -29,6 +29,8 @@ data class FlyToStatus( val state: FlyToMissionState = FlyToMissionState.UNKNOWN, val mode: FlyToMode = FlyToMode.UNKNOWN, val height: Int = 0, + val heightMin: Int = 1, + val heightMax: Int = 1500, val targetLatitude: Double = 0.0, val targetLongitude: Double = 0.0, val targetAltitude: Double = 0.0, @@ -74,8 +76,12 @@ class FlyToService { private val capabilityListener = object : IMissionCapabilityListener { override fun onMissionCapabilityUpdate(capability: FlyToCapability) { + val heightMin = capability.heightRange?.min?.roundToInt()?.coerceAtLeast(1) ?: 1 + val heightMax = capability.heightRange?.max?.roundToInt()?.coerceAtLeast(heightMin) ?: 1500 _status.update { it.copy( + heightMin = heightMin, + heightMax = heightMax, heightRangeText = capability.heightRange?.toString().orEmpty() ) } @@ -107,26 +113,24 @@ class FlyToService { DjiCommandResult.failed(SafetyInterlock.LOCKED_MESSAGE) } else { suspendCancellableCoroutine { continuation -> - val targetHeight = height.roundToInt().coerceAtLeast(1) + val targetHeight = height.normalizedFlyToHeight() val safeTakeoffHeight = securityTakeoffHeight .coerceAtLeast(1) .coerceAtMost(targetHeight) val target = FlyToTarget().apply { - targetLocation = LocationCoordinate3D(latitude, longitude, height) + targetLocation = LocationCoordinate3D(latitude, longitude, targetHeight.toDouble()) this.maxSpeed = maxSpeed this.securityTakeoffHeight = safeTakeoffHeight } - val param = FlyToParam().apply { - this.flyToMode = flyToMode - this.height = targetHeight - } Log.d( FlyToLogTag, - "startFlyTo lat=$latitude lon=$longitude height=$height mode=$flyToMode maxSpeed=$maxSpeed securityTakeoffHeight=$safeTakeoffHeight" + "startFlyTo lat=$latitude lon=$longitude requestedHeight=$height targetHeight=$targetHeight " + + "mode=$flyToMode maxSpeed=$maxSpeed securityTakeoffHeight=$safeTakeoffHeight " + + "heightRange=${status.value.heightMin}..${status.value.heightMax}" ) manager.startMission( target, - param, + null, callback( continuation, "指点飞行已开始", @@ -138,7 +142,7 @@ class FlyToService { height = targetHeight, targetLatitude = latitude, targetLongitude = longitude, - targetAltitude = height + targetAltitude = targetHeight.toDouble() ) } }, @@ -172,18 +176,20 @@ class FlyToService { DjiCommandResult.failed(SafetyInterlock.LOCKED_MESSAGE) } else { suspendCancellableCoroutine { continuation -> - val targetHeight = height.roundToInt().coerceAtLeast(1) + val targetHeight = height.normalizedFlyToHeight() val safeTakeoffHeight = securityTakeoffHeight .coerceAtLeast(1) .coerceAtMost(targetHeight) val target = FlyToTarget().apply { - targetLocation = LocationCoordinate3D(latitude, longitude, height) + targetLocation = LocationCoordinate3D(latitude, longitude, targetHeight.toDouble()) this.maxSpeed = maxSpeed this.securityTakeoffHeight = safeTakeoffHeight } Log.d( FlyToLogTag, - "updateFlyToTarget lat=$latitude lon=$longitude height=$height maxSpeed=$maxSpeed securityTakeoffHeight=$safeTakeoffHeight" + "updateFlyToTarget lat=$latitude lon=$longitude requestedHeight=$height targetHeight=$targetHeight " + + "maxSpeed=$maxSpeed securityTakeoffHeight=$safeTakeoffHeight " + + "heightRange=${status.value.heightMin}..${status.value.heightMax}" ) manager.updateMissionTarget( target, @@ -195,7 +201,7 @@ class FlyToService { it.copy( targetLatitude = latitude, targetLongitude = longitude, - targetAltitude = height + targetAltitude = targetHeight.toDouble() ) } } @@ -214,7 +220,7 @@ class FlyToService { suspendCancellableCoroutine { continuation -> val param = FlyToParam().apply { if (mode != null) flyToMode = mode - if (height != null) this.height = height.coerceAtLeast(1) + if (height != null) this.height = height.toDouble().normalizedFlyToHeight() } manager.updateMissionParam( param, @@ -234,10 +240,11 @@ class FlyToService { suspend fun setFlyToHeight(height: Int): DjiCommandResult = suspendCancellableCoroutine { continuation -> - val param = FlyToParam().apply { this.height = height } + val targetHeight = height.toDouble().normalizedFlyToHeight() + val param = FlyToParam().apply { this.height = targetHeight } manager.updateMissionParam( param, - callback(continuation, "指点飞行高度已更新：${height}m") + callback(continuation, "指点飞行高度已更新：${targetHeight}m") ) } @@ -255,7 +262,15 @@ class FlyToService { override fun onFailure(error: IDJIError) { onFailure() + Log.w(FlyToLogTag, "$successMessage failed: $error") continuation.resume(DjiCommandResult.failed(error)) } } + + private fun Double.normalizedFlyToHeight(): Int { + val range = status.value + return roundToInt() + .coerceAtLeast(range.heightMin) + .coerceAtMost(range.heightMax) + } } 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 f7dbabd..5a4e025 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 kotlinx.coroutines.delay import kotlinx.coroutines.flow.MutableStateFlow import kotlinx.coroutines.flow.StateFlow import kotlinx.coroutines.flow.asStateFlow @@ -58,17 +59,35 @@ class LiveStreamingService { private var listenerRegistered = false private var lastConfig: LiveStartConfig? = null + private var availableCameraIndexes: List = emptyList() + + private val cameraListener = object : ICameraStreamManager.AvailableCameraUpdatedListener { + override fun onAvailableCameraUpdated(cameras: List) { + availableCameraIndexes = cameras + Log.d(LiveStreamLogTag, "available camera streams=$cameras") + } + + override fun onCameraStreamEnableUpdate(enableMap: Map) { + Log.d(LiveStreamLogTag, "camera stream enable=$enableMap") + } + } private val statusListener = object : LiveStreamStatusListener { override fun onLiveStreamStatusUpdate(status: LiveStreamStatus?) { val resolution = status?.resolution?.let { "${it.width}x${it.height}" }.orEmpty() + val fps = status?.fps ?: 0 + val vbps = status?.vbps ?: 0 _state.update { it.copy( streaming = status?.isStreaming ?: liveStreamManager().isStreaming, - fps = status?.fps ?: 0, - vbps = status?.vbps ?: 0, + fps = fps, + vbps = vbps, resolution = resolution, - message = if (status?.isStreaming == true) "直播推流中" else it.message, + message = when { + status?.isStreaming != true -> it.message + fps <= 0 || vbps <= 0 -> "直播已开启，但暂未收到视频帧" + else -> "直播推流中" + }, lastError = null ) } @@ -89,6 +108,7 @@ class LiveStreamingService { fun clear() { if (listenerRegistered) { runCatching { liveStreamManager().removeLiveStreamStatusListener(statusListener) } + runCatching { cameraStreamManager().removeAvailableCameraUpdatedListener(cameraListener) } listenerRegistered = false } } @@ -163,7 +183,8 @@ class LiveStreamingService { if (!stopResult.success) return stopResult } - val cameraIndex = config.videoId.toCameraIndex() + waitForAvailableCameraStreams() + val cameraIndex = selectCameraIndex(config.videoId) runCatching { manager.cameraIndex = cameraIndex cameraStreamManager().enableStream(cameraIndex, true) @@ -183,7 +204,8 @@ class LiveStreamingService { Log.d( LiveStreamLogTag, - "start live stream manual=$manual url=${config.url} videoId=${config.videoId} quality=${config.quality} camera=$cameraIndex" + "start live stream manual=$manual url=${config.url} videoId=${config.videoId} " + + "quality=${config.quality} camera=$cameraIndex available=$availableCameraIndexes" ) _state.update { it.copy( @@ -215,12 +237,37 @@ class LiveStreamingService { private fun ensureListener() { if (listenerRegistered) return liveStreamManager().addLiveStreamStatusListener(statusListener) + cameraStreamManager().addAvailableCameraUpdatedListener(cameraListener) listenerRegistered = true } private fun liveStreamManager() = MediaDataCenter.getInstance().liveStreamManager private fun cameraStreamManager() = MediaDataCenter.getInstance().cameraStreamManager + + private suspend fun waitForAvailableCameraStreams(timeoutMs: Long = 1_500L) { + val startAt = System.currentTimeMillis() + while (availableCameraIndexes.isEmpty() && System.currentTimeMillis() - startAt < timeoutMs) { + delay(100L) + } + } + + private fun selectCameraIndex(videoId: String): ComponentIndexType { + val explicitIndex = videoId.toExplicitCameraIndex() + val available = availableCameraIndexes + if (explicitIndex != null && (available.isEmpty() || available.contains(explicitIndex))) { + return explicitIndex + } + if (videoId.hasCloudPayloadIndex() && available.isNotEmpty()) { + return available.firstNonFpvOrFirst() + } + return when { + available.contains(ComponentIndexType.LEFT_OR_MAIN) -> ComponentIndexType.LEFT_OR_MAIN + available.isNotEmpty() -> available.firstNonFpvOrFirst() + explicitIndex != null -> explicitIndex + else -> ComponentIndexType.LEFT_OR_MAIN + } + } } private fun JSONObject.toLiveStartConfig(): LiveStartConfig? { @@ -379,9 +426,20 @@ private fun Int.toStreamQuality(): StreamQuality = else -> StreamQuality.HD } -private fun String.toCameraIndex(): ComponentIndexType = +private fun String.toExplicitCameraIndex(): ComponentIndexType? = when { contains("/FPV", ignoreCase = true) -> ComponentIndexType.FPV contains("/right", ignoreCase = true) -> ComponentIndexType.RIGHT - else -> ComponentIndexType.LEFT_OR_MAIN + contains("/up", ignoreCase = true) -> ComponentIndexType.UP + contains("/port_1", ignoreCase = true) || contains("/port1", ignoreCase = true) -> ComponentIndexType.PORT_1 + contains("/port_2", ignoreCase = true) || contains("/port2", ignoreCase = true) -> ComponentIndexType.PORT_2 + contains("/port_3", ignoreCase = true) || contains("/port3", ignoreCase = true) -> ComponentIndexType.PORT_3 + contains("/port_4", ignoreCase = true) || contains("/port4", ignoreCase = true) -> ComponentIndexType.PORT_4 + else -> null } + +private fun String.hasCloudPayloadIndex(): Boolean = + Regex("""\d+-\d+-\d+""").containsMatchIn(this) + +private fun List.firstNonFpvOrFirst(): ComponentIndexType = + firstOrNull { it != ComponentIndexType.FPV } ?: first() 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 c86870a..6fccf09 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 @@ -18,8 +18,10 @@ import dji.sdk.keyvalue.key.DJIGimbalKey import dji.sdk.keyvalue.key.DJIKey import dji.sdk.keyvalue.key.FlightControllerKey import dji.sdk.keyvalue.key.KeyTools +import dji.sdk.keyvalue.key.PayloadKey import dji.sdk.keyvalue.key.ProductKey import dji.sdk.keyvalue.key.RemoteControllerKey +import dji.sdk.keyvalue.value.camera.CameraType import dji.sdk.keyvalue.key.RtkMobileStationKey import dji.sdk.keyvalue.value.camera.CameraWorkMode import dji.sdk.keyvalue.value.camera.CameraMode @@ -37,8 +39,11 @@ import dji.sdk.keyvalue.value.flightcontroller.GPSSignalLevel import dji.sdk.keyvalue.value.flightcontroller.HeightAboveSeaLevelMsg import dji.sdk.keyvalue.value.flightcontroller.RemoteControllerFlightMode import dji.sdk.keyvalue.value.flightcontroller.WindDirection +import dji.sdk.keyvalue.value.payload.PayloadCameraType +import dji.sdk.keyvalue.value.product.ProductType import dji.sdk.keyvalue.value.remotecontroller.BatteryInfo import dji.sdk.keyvalue.value.remotecontroller.RcGPSInfo +import dji.sdk.keyvalue.value.remotecontroller.RemoteControllerType import dji.sdk.keyvalue.value.rtkbasestation.RTKReferenceStationSource import dji.sdk.keyvalue.value.rtkbasestation.RTKServiceState import dji.sdk.keyvalue.value.rtkmobilestation.RTKLocation @@ -64,6 +69,21 @@ import kotlinx.coroutines.flow.update private const val TelemetryLogTag = "ZklhTelemetry" private const val RTK_START_RETRY_INTERVAL_MS = 20_000L private const val RTK_BOOTSTRAP_RETRY_INTERVAL_MS = 15_000L +// Telemetry must not change the aircraft RTK configuration. In particular, an +// enabled-but-unfixed M350 RTK module prevents waypoint takeoff. +private const val AutoManageRtk = false +// Restore only the already-selected network RTK service. This mirrors the +// recovery performed by DJI Pilot without force-enabling RTK, changing its +// source, or changing the aircraft's maintain-accuracy setting. +private const val RestoreExistingNetworkRtkService = true + +data class TelemetryCameraIdentity( + val component: String, + val cameraType: String = "", + val cameraTypeValue: Int = 0, + val payloadCameraType: String = "", + val payloadCameraTypeValue: Int = 0 +) data class TelemetrySnapshot( val latitude: Double = 0.0, @@ -119,6 +139,18 @@ data class TelemetrySnapshot( val windDirection: Int = 0, val windSpeed: Int = 0, val remainingFlightTime: Int = 0, + val productType: String = "", + val productTypeValue: Int = 0, + val remoteControllerType: String = "", + val remoteControllerTypeValue: Int = 0, + val groundDeviceIdentity: Int = 0, + val uavDeviceIdentity: Int = 0, + val cameraType: String = "", + val cameraTypeValue: Int = 0, + val payloadCameraType: String = "", + val payloadCameraTypeValue: Int = 0, + val cameraComponent: String = ComponentIndexType.LEFT_OR_MAIN.name, + val cameraIdentities: List = emptyList(), val batteryPercentNeededToLand: Int = 7, val batteryPercentNeededToLandKnown: Boolean = false, val batteryPercentNeededToGoHome: Int = 14, @@ -198,7 +230,11 @@ class TelemetryRepository( -> { rtkStartInProgress = false rtkServiceStarted = false - ensureRtkServiceStarted(lastRtkSystemState) + if (AutoManageRtk) { + ensureRtkServiceStarted(lastRtkSystemState) + } else if (RestoreExistingNetworkRtkService) { + ensureExistingNetworkRtkServiceStarted(lastRtkSystemState) + } } else -> { // READY/CONNECTING/PROCESSING are transitional states; keep the current flags. @@ -210,7 +246,11 @@ class TelemetryRepository( Log.w(TelemetryLogTag, "RTKNetworkServiceError=$error") rtkStartInProgress = false rtkServiceStarted = false - ensureRtkServiceStarted(lastRtkSystemState) + if (AutoManageRtk) { + ensureRtkServiceStarted(lastRtkSystemState) + } else if (RestoreExistingNetworkRtkService) { + ensureExistingNetworkRtkServiceStarted(lastRtkSystemState) + } } } private val _state = MutableStateFlow(TelemetrySnapshot()) @@ -288,6 +328,42 @@ class TelemetryRepository( _state.update { it.copy(firmwareVersion = value, error = null) } } } + listenSafely("KeyProductType", KeyTools.createKey(ProductKey.KeyProductType)) { productType: ProductType? -> + val value = productType?.name.orEmpty() + if (value.isNotBlank()) { + val typeValue = productType?.value() ?: 0 + _state.update { it.copy(productType = value, productTypeValue = typeValue, error = null) } + Log.d(TelemetryLogTag, "KeyProductType=$value value=$typeValue") + } + } + listenSafely("KeyRemoteControllerType", KeyTools.createKey(RemoteControllerKey.KeyRemoteControllerType)) { type: RemoteControllerType? -> + val value = type?.name.orEmpty() + if (value.isNotBlank()) { + val typeValue = type?.value() ?: 0 + _state.update { + it.copy( + remoteControllerType = value, + remoteControllerTypeValue = typeValue, + error = null + ) + } + Log.d(TelemetryLogTag, "KeyRemoteControllerType=$value value=$typeValue") + } + } + listenSafely("KeyGroundDeviceIdentity", KeyTools.createKey(RemoteControllerKey.KeyGroundDeviceIdentity)) { value: Int? -> + val identity = value ?: 0 + if (identity > 0) { + _state.update { it.copy(groundDeviceIdentity = identity, error = null) } + Log.d(TelemetryLogTag, "KeyGroundDeviceIdentity=$identity") + } + } + listenSafely("KeyUAVDeviceIdentity", KeyTools.createKey(RemoteControllerKey.KeyUAVDeviceIdentity)) { value: Int? -> + val identity = value ?: 0 + if (identity > 0) { + _state.update { it.copy(uavDeviceIdentity = identity, error = null) } + Log.d(TelemetryLogTag, "KeyUAVDeviceIdentity=$identity") + } + } listenSafely("KeyFirmwareVersion", KeyTools.createKey(FlightControllerKey.KeyFirmwareVersion)) { firmware: String? -> val value = firmware.orEmpty() if (value.isNotBlank()) { @@ -502,6 +578,7 @@ class TelemetryRepository( listenRemoteControllerTelemetry() startAndroidLocationFallback() listenCameraTelemetry() + listenAdditionalCameraIdentities() listenGimbalTelemetry() listenRtkTelemetry() } @@ -533,6 +610,7 @@ class TelemetryRepository( } private fun listenCameraTelemetry(cameraIndex: ComponentIndexType = ComponentIndexType.LEFT_OR_MAIN) { + listenCameraIdentity(cameraIndex) listenSafely("KeyCameraWorkMode", KeyTools.createKey(DJICameraKey.KeyCameraWorkMode, cameraIndex)) { mode: CameraWorkMode? -> _state.update { it.copy(cameraWorkMode = mode?.name.orEmpty(), error = null) } } @@ -595,6 +673,69 @@ class TelemetryRepository( } } + private fun listenAdditionalCameraIdentities() { + listOf( + ComponentIndexType.RIGHT, + ComponentIndexType.UP, + ComponentIndexType.PORT_1, + ComponentIndexType.PORT_2, + ComponentIndexType.PORT_3, + ComponentIndexType.PORT_4 + ).forEach { listenCameraIdentity(it) } + } + + private fun listenCameraIdentity(cameraIndex: ComponentIndexType) { + listenSafely("KeyCameraType[$cameraIndex]", KeyTools.createKey(CameraKey.KeyCameraType, cameraIndex)) { type: CameraType? -> + val value = type?.name.orEmpty() + if (value.isBlank() || value == CameraType.NOT_SUPPORTED.name) return@listenSafely + val typeValue = type?.value() ?: 0 + updateCameraIdentity(cameraIndex, cameraType = value, cameraTypeValue = typeValue) + Log.d(TelemetryLogTag, "KeyCameraType camera=$cameraIndex type=$value value=$typeValue") + } + listenSafely( + "KeyPayloadCameraType[$cameraIndex]", + KeyTools.createKey(PayloadKey.KeyPayloadCameraType, cameraIndex) + ) { type: PayloadCameraType? -> + val value = type?.name.orEmpty() + if (value.isBlank() || value == PayloadCameraType.UNKNOWN.name) return@listenSafely + val typeValue = type?.value() ?: 0 + updateCameraIdentity(cameraIndex, payloadCameraType = value, payloadCameraTypeValue = typeValue) + Log.d(TelemetryLogTag, "KeyPayloadCameraType camera=$cameraIndex type=$value value=$typeValue") + } + } + + private fun updateCameraIdentity( + cameraIndex: ComponentIndexType, + cameraType: String? = null, + cameraTypeValue: Int? = null, + payloadCameraType: String? = null, + payloadCameraTypeValue: Int? = null + ) { + val component = cameraIndex.name + _state.update { snapshot -> + val identities = snapshot.cameraIdentities + .associateBy { it.component } + .toMutableMap() + val previous = identities[component] ?: TelemetryCameraIdentity(component = component) + val updated = previous.copy( + cameraType = cameraType ?: previous.cameraType, + cameraTypeValue = cameraTypeValue?.takeIf { it > 0 } ?: previous.cameraTypeValue, + payloadCameraType = payloadCameraType ?: previous.payloadCameraType, + payloadCameraTypeValue = payloadCameraTypeValue?.takeIf { it > 0 } ?: previous.payloadCameraTypeValue + ) + identities[component] = updated + snapshot.copy( + cameraType = updated.cameraType.ifBlank { snapshot.cameraType }, + cameraTypeValue = updated.cameraTypeValue.takeIf { it > 0 } ?: snapshot.cameraTypeValue, + payloadCameraType = updated.payloadCameraType.ifBlank { snapshot.payloadCameraType }, + payloadCameraTypeValue = updated.payloadCameraTypeValue.takeIf { it > 0 } ?: snapshot.payloadCameraTypeValue, + cameraComponent = component, + cameraIdentities = identities.values.sortedBy { it.component.cameraComponentSortOrder() }, + error = null + ) + } + } + private fun listenRemoteControllerTelemetry() { listenSafely("KeyRcGPSInfo", KeyTools.createKey(RemoteControllerKey.KeyRcGPSInfo)) { info: RcGPSInfo? -> val location = info?.location @@ -671,7 +812,11 @@ class TelemetryRepository( } listenRtkStationTelemetry() listenRtkMobileStationKeys() - bootstrapAllPositioning("telemetry_start") + if (AutoManageRtk) { + bootstrapAllPositioning("telemetry_start") + } else { + Log.d(TelemetryLogTag, "RTK telemetry is observe-only; skip automatic RTK enable/source/service changes") + } } private fun listenRtkStationTelemetry() { @@ -829,7 +974,32 @@ class TelemetryRepository( error = null ) } - ensureRtkServiceStarted(state) + if (AutoManageRtk) { + ensureRtkServiceStarted(state) + } else if (RestoreExistingNetworkRtkService) { + ensureExistingNetworkRtkServiceStarted(state) + } + } + + /** + * Only restart the network service selected by the operator/Pilot. This + * must never enable RTK, change reference source, or force accuracy mode: + * those operations can make an M350 reject takeoff while RTK is unfixed. + */ + private fun ensureExistingNetworkRtkServiceStarted(state: RTKSystemState?) { + if (state?.isRTKEnabled != true || state.rtkHealthy) { + if (state?.rtkHealthy == true) { + rtkStartInProgress = false + rtkServiceStarted = true + } + return + } + val source = state.rtkReferenceStationSource ?: return + if (!source.isNetworkRtkSource() || rtkServiceStarted) return + val now = System.currentTimeMillis() + if (rtkStartInProgress || now - lastRtkStartAtMs < RTK_START_RETRY_INTERVAL_MS) return + Log.d(TelemetryLogTag, "restore existing RTK network service source=$source") + startNetworkRtkService(source, setMaintainAccuracy = false) } private fun ensureRtkServiceStarted(state: RTKSystemState?) { @@ -933,7 +1103,10 @@ class TelemetryRepository( ) } - private fun startNetworkRtkService(source: RTKReferenceStationSource) { + private fun startNetworkRtkService( + source: RTKReferenceStationSource, + setMaintainAccuracy: Boolean = true + ) { val now = System.currentTimeMillis() if (now - lastRtkStartAtMs < 1_000L && source == lastRtkStartSource) { Log.d(TelemetryLogTag, "skip duplicate RTK network service source=$source") @@ -943,7 +1116,9 @@ class TelemetryRepository( rtkServiceStarted = false lastRtkStartAtMs = now lastRtkStartSource = source - rtkCenter.setRTKMaintainAccuracyEnabled(true, null) + if (setMaintainAccuracy) { + rtkCenter.setRTKMaintainAccuracyEnabled(true, null) + } Log.d(TelemetryLogTag, "start RTK network service source=$source coordinate=$DefaultNetworkRtkCoordinateSystem") when (source) { RTKReferenceStationSource.QX_NETWORK_SERVICE -> { @@ -1096,6 +1271,18 @@ class TelemetryRepository( } +private fun String.cameraComponentSortOrder(): Int = + when (uppercase()) { + ComponentIndexType.LEFT_OR_MAIN.name -> 0 + ComponentIndexType.RIGHT.name -> 1 + ComponentIndexType.UP.name -> 2 + ComponentIndexType.PORT_1.name -> 3 + ComponentIndexType.PORT_2.name -> 4 + ComponentIndexType.PORT_3.name -> 5 + ComponentIndexType.PORT_4.name -> 6 + else -> 100 + } + private fun isValidCoordinate(latitude: Double?, longitude: Double?): Boolean { val lat = latitude ?: return false val lon = longitude ?: return false diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRtkStatus.kt b/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRtkStatus.kt new file mode 100644 index 0000000..1dd1a1d --- /dev/null +++ b/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRtkStatus.kt @@ -0,0 +1,24 @@ +package com.zklh.dronecontroller.core.telemetry + +import java.util.Locale + +fun TelemetrySnapshot.hasReliableRtkPosition(): Boolean = + rtkLocationValid && + (rtkHealthy || rtkFusionDataUsable || rtkPositioningSolution.isSolvedRtkSolution()) + +fun TelemetrySnapshot.isRtkBlockingTakeoff(): Boolean = + rtkEnabled && !hasReliableRtkPosition() + +fun TelemetrySnapshot.isRtkBlockingWaypointMission(): Boolean = + isRtkBlockingTakeoff() + +private fun String.isSolvedRtkSolution(): Boolean { + val value = trim() + .replace("-", "_") + .replace(".", "_") + .lowercase(Locale.US) + return value == "fixed_point" || + value == "float_point" || + value == "fixed" || + value == "float" +} 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 3173197..482e20f 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt @@ -172,6 +172,7 @@ import com.zklh.dronecontroller.core.safety.SafetyInterlock import com.zklh.dronecontroller.core.simulator.SimulatorService import com.zklh.dronecontroller.core.telemetry.TelemetryRepository import com.zklh.dronecontroller.core.telemetry.TelemetrySnapshot +import com.zklh.dronecontroller.core.telemetry.hasReliableRtkPosition import com.zklh.dronecontroller.core.video.DjiVideoPreviewService import com.zklh.dronecontroller.core.virtualstick.StickPosition import com.zklh.dronecontroller.core.virtualstick.VirtualStickService @@ -310,22 +311,22 @@ fun DroneControllerScreen() { val appContext = context.applicationContext val savedCloudLoginConfig = remember(appContext) { appContext.readSavedCloudLoginConfig() } val sdkState by DroneSdkManager.state.collectAsState() - val flightControl = remember { FlightControlService() } + val telemetryRepository = remember(context) { TelemetryRepository(context.applicationContext) } + val telemetry by telemetryRepository.state.collectAsState() + val latestTelemetry by rememberUpdatedState(telemetry) + val flightControl = remember { FlightControlService { latestTelemetry } } val virtualStick = remember { VirtualStickService() } val waypointMission = remember { WaypointMissionService() } val cameraMedia = remember { CameraMediaService() } val flyToService = remember { FlyToService() } val liveStreaming = remember { LiveStreamingService() } val simulator = remember { SimulatorService() } - val telemetryRepository = remember(context) { TelemetryRepository(context.applicationContext) } val warningRepository = remember { DroneWarningRepository() } val cloudLoginClient = remember { CloudLoginClient() } val cloudMqttService = remember { CloudMqttService(cloudLoginClient) } val cloudMediaUpload = remember(appContext, cloudMqttService) { CloudMediaUploadService(appContext) { cloudMqttService.state.value.session } } - val telemetry by telemetryRepository.state.collectAsState() - val latestTelemetry by rememberUpdatedState(telemetry) val cloudCommandExecutor = remember(flightControl, cameraMedia, flyToService, liveStreaming, virtualStick, waypointMission, cloudMediaUpload) { CloudCommandExecutor( flightControl = flightControl, @@ -335,7 +336,8 @@ fun DroneControllerScreen() { virtualStick = virtualStick, waypointMission = waypointMission, mediaUpload = cloudMediaUpload, - telemetryProvider = { latestTelemetry } + telemetryProvider = { latestTelemetry }, + progressPublisher = cloudMqttService ) } val stickStatus by virtualStick.status.collectAsState() @@ -2209,7 +2211,7 @@ private fun PreflightScreen( StatusChip(Icons.Filled.BatteryFull, batteryStatusText(sdkState.productConnected, telemetry), if (sdkState.productConnected) Color.Black else PilotMuted) StatusChip(Icons.Filled.Radio, if (sdkState.productConnected) "100%" else "--", if (sdkState.productConnected) Color.Black else PilotMuted) StatusChip(Icons.Filled.Home, if (sdkState.productConnected) "未设置" else "--", if (sdkState.productConnected) PilotOrange else PilotMuted) - StatusChip(Icons.Filled.Warning, if (sdkState.productConnected) "RTK 未连接" else "RTK --", if (sdkState.productConnected) PilotDanger else PilotMuted) + StatusChip(Icons.Filled.Warning, rtkStatusText(sdkState.productConnected, telemetry), rtkStatusColor(sdkState.productConnected, telemetry)) StatusChip(Icons.Filled.UploadFile, if (sdkState.productConnected) "39.8G" else "--", if (sdkState.productConnected) Color.Black else PilotMuted) StatusChip(Icons.Filled.Person, "A控", Color.Black) } @@ -3002,6 +3004,8 @@ private fun FlightTopBar( val batteryPercent = batteryPercentText(sdkState.productConnected, telemetry) val batteryVoltage = batteryVoltageText(sdkState.productConnected, telemetry) val alertCount = effectiveAlertCount(alertState) + val rtkColor = rtkStatusColor(sdkState.productConnected, telemetry) + val rtkCount = rtkSatelliteText(sdkState.productConnected, telemetry) Column(modifier.background(PilotBlack)) { Row( @@ -3062,8 +3066,8 @@ private fun FlightTopBar( modifier = Modifier.weight(1f) ) Column(horizontalAlignment = Alignment.CenterHorizontally, modifier = Modifier.width(44.dp)) { - Text("RTK", color = if (sdkState.productConnected) PilotDanger else Color(0xFF9CA3AF), fontSize = 14.sp, fontWeight = FontWeight.Black) - Text(if (sdkState.productConnected) "0" else "--", color = if (sdkState.productConnected) PilotDanger else Color(0xFF9CA3AF), fontSize = 15.sp, fontWeight = FontWeight.Black) + Text("RTK", color = rtkColor, fontSize = 14.sp, fontWeight = FontWeight.Black) + Text(rtkCount, color = rtkColor, fontSize = 15.sp, fontWeight = FontWeight.Black) } Text(if (sdkState.productLinkConnected) "RC 在线" else "RC --", color = Color.White, fontSize = 15.sp, fontWeight = FontWeight.Black, modifier = Modifier.width(72.dp)) Row(verticalAlignment = Alignment.CenterVertically, modifier = Modifier.width(68.dp)) { @@ -4240,6 +4244,31 @@ private fun batteryVoltageText(connected: Boolean, telemetry: TelemetrySnapshot) "--" } +private fun rtkStatusText(connected: Boolean, telemetry: TelemetrySnapshot): String = + when { + !connected -> "RTK --" + !telemetry.rtkEnabled -> "RTK 关" + telemetry.hasReliableRtkPosition() -> "RTK 正常" + telemetry.rtkWorking || telemetry.rtkBeingUsed || telemetry.rtkSatelliteCount > 0 -> "RTK 收敛" + else -> "RTK 未就绪" + } + +private fun rtkStatusColor(connected: Boolean, telemetry: TelemetrySnapshot): Color = + when { + !connected -> PilotMuted + !telemetry.rtkEnabled -> PilotMuted + telemetry.hasReliableRtkPosition() -> PilotGreen + telemetry.rtkWorking || telemetry.rtkBeingUsed || telemetry.rtkSatelliteCount > 0 -> PilotOrange + else -> PilotDanger + } + +private fun rtkSatelliteText(connected: Boolean, telemetry: TelemetrySnapshot): String = + when { + !connected -> "--" + !telemetry.rtkEnabled -> "关" + else -> telemetry.rtkSatelliteCount.coerceAtLeast(0).toString() + } + private fun effectiveCommandMessage(sdkState: DroneSdkState, rawMessage: String): String { val passiveMessage = rawMessage.isBlank() || rawMessage.contains("等待设备") ||