val navTask = object : Runnable {
override fun run() {
val currentLoc = getAircraftLocation()
val remainingDistance = calculateDistance(
currentLoc.latitude,
currentLoc.longitude,
targetLat,
targetLon
)
if (remainingDistance < ARRIVAL_THRESHOLD) {
isNavigating = false
startDynamicAdjustment()
} else {
sendNavigationCommand()
virtualStickHandler?.postDelayed(this, 100)
}
}
}
var downwardObstacleDisabled = false
if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
downwardObstacleDisabled = true
setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
}
private fun setObstacleAvoidanceEnable(enabled: Boolean, direction: PerceptionDirection) {
if (direction == null) {
Log.e("Perception", "方向参数为空,无法设置避障")
return
}
PerceptionManager.getInstance().setObstacleAvoidanceEnabled(
enabled, direction, object : CommonCallbacks.CompletionCallback {
override fun onSuccess() {
toastResult?.postValue(DJIToastResult.success("成功设置【${direction.name}】方向的避障为:${if(enabled)"开启"else"关闭"}"))
Log.i("Perception", "成功设置【${direction.name}】方向的避障为:${if(enabled)"开启"else"关闭"}")
}
override fun onFailure(error: IDJIError) {
downwardObstacleDisabled = false
toastResult?.postValue(DJIToastResult.failed("设置【${direction.name}】方向的避障失败:$error"))
Log.e("Perception", "设置【${direction.name}】方向的避障失败:$error")
}
}
)
}
private fun startDynamicAdjustment() {
isAdjusting = true
virtualStickHandler = Handler(Looper.getMainLooper())
val adjustTask = object : Runnable {
override fun run() {
if (!isAdjusting) return
val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
val currentXOffsetAbs = Math.abs(xOffset)
val currentYOffsetAbs = Math.abs(yOffset)
if (currentAltitude <= 0.1) {
stopLanding()
return
}
if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
downwardObstacleDisabled = true
setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
}
val offsetThreshold = getOffsetThreshold(currentAltitude)
val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
val adjustParam = VirtualStickFlightControlParam().apply {
rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
verticalControlMode = VerticalControlMode.VELOCITY
rollPitchControlMode = RollPitchControlMode.VELOCITY
roll = if (currentXOffsetAbs > offsetThreshold) {
if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
} else 0.0
pitch = if (currentYOffsetAbs > offsetThreshold) {
if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
} else 0.0
verticalThrottle = -descentSpeed
}
VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
virtualStickHandler?.postDelayed(this, 100)
}
}
virtualStickHandler?.post(adjustTask)
}
fun returnToVehicle(callback: CommonCallbacks.CompletionCallback) {
val aircraftLocation = getAircraftLocation()
if (aircraftLocation == null || !isLocationValid(aircraftLocation.latitude, aircraftLocation.longitude)) {
callback.onFailure(DJICommonError.FACTORY.build("无法获取无人机位置信息"))
return
}
val vehicleLatitude = 22.579
val vehicleLongitude = 113.941
val distance = calculateDistance(
aircraftLocation.latitude,
aircraftLocation.longitude,
vehicleLatitude,
vehicleLongitude
)
if (distance > 500) {
callback.onFailure(DJICommonError.FACTORY.build("距离过远:${String.format("%.2f", distance)}m, 超出 500m 限制"))
return
}
toastResult?.postValue(DJIToastResult.success("开始飞向车辆位置"))
navigateToTarget(vehicleLatitude, vehicleLongitude, 100.0, callback)
}
private fun navigateToTarget(
targetLat: Double,
targetLon: Double,
targetAlt: Double,
callback: CommonCallbacks.CompletionCallback
) {
VirtualStickManager.getInstance().enableVirtualStick(object : CommonCallbacks.CompletionCallback {
override fun onSuccess() {
VirtualStickManager.getInstance().setVirtualStickAdvancedModeEnabled(true)
isNavigating = true
startNavigation(targetLat, targetLon, targetAlt, callback)
}
override fun onFailure(error: IDJIError) {
callback.onFailure(error)
}
})
}
private fun startNavigation(
targetLat: Double,
targetLon: Double,
targetAlt: Double,
callback: CommonCallbacks.CompletionCallback
) {
virtualStickHandler = Handler(Looper.getMainLooper())
val navTask = object : Runnable {
override fun run() {
if (!isNavigating) return
val currentLoc = getAircraftLocation()
if (currentLoc == null) {
virtualStickHandler?.postDelayed(this, 100)
return
}
val remainingDistance = calculateDistance(
currentLoc.latitude,
currentLoc.longitude,
targetLat,
targetLon
)
println("targetLat:" + targetLat + " targetLon:" + targetLon + " currentLoc.latitude:" + currentLoc.latitude + " currentLoc.longitude:" + currentLoc.longitude + " remainingDistance:" + remainingDistance)
if (remainingDistance < ARRIVAL_THRESHOLD) {
isNavigating = false
virtualStickHandler?.removeCallbacksAndMessages(null)
callback.onSuccess()
toastResult?.postValue(DJIToastResult.success("已到达车辆位置,开始精确定位"))
startGimbalAngleRotation(GimbalAngleRotationMode.ABSOLUTE_ANGLE, -90.0, 0.0, 0.0, 1.0)
startDynamicAdjustment()
} else {
val bearing = calculateBearing(
currentLoc.latitude,
currentLoc.longitude,
targetLat,
targetLon
)
val navParam = VirtualStickFlightControlParam().apply {
rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND
verticalControlMode = VerticalControlMode.POSITION
yawControlMode = YawControlMode.ANGLE
rollPitchControlMode = RollPitchControlMode.VELOCITY
val bearingRad = Math.toRadians(bearing)
pitch = NAVIGATION_SPEED * Math.sin(bearingRad)
roll = NAVIGATION_SPEED * Math.cos(bearingRad)
yaw = bearing
verticalThrottle = targetAlt
}
VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(navParam)
virtualStickHandler?.postDelayed(this, 100)
}
}
}
virtualStickHandler?.post(navTask)
}
fun startGimbalAngleRotation(
mode: GimbalAngleRotationMode,
pitch: Double,
yaw: Double,
roll: Double,
duration: Double
) {
val rotation = GimbalAngleRotation().apply {
setMode(mode)
setPitch(pitch)
setYaw(yaw)
setRoll(roll)
setDuration(duration)
}
KeyManager.getInstance().performAction(
KeyTools.createKey(GimbalKey.KeyRotateByAngle),
rotation,
object : CommonCallbacks.CompletionCallbackWithParam<EmptyMsg> {
override fun onSuccess(result: EmptyMsg?) {
toastResult?.postValue(DJIToastResult.success("云台旋转成功"))
Log.i("Gimbal", "云台旋转成功:yaw:${rotation.yaw},pitch:${rotation.pitch},roll:${rotation.roll}")
}
override fun onFailure(error: IDJIError) {
toastResult?.postValue(DJIToastResult.failed("云台旋转失败,$error"))
Log.e("Gimbal", "云台旋转失败,$error")
}
}
)
}
private fun startDynamicAdjustment() {
isAdjusting = true
virtualStickHandler = Handler(Looper.getMainLooper())
val adjustTask = object : Runnable {
override fun run() {
if (!isAdjusting) return
val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
val currentXOffsetAbs = Math.abs(xOffset)
val currentYOffsetAbs = Math.abs(yOffset)
if (currentAltitude <= 5 && !downwardObstacleDisabled) {
downwardObstacleDisabled = true
setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
}
if (currentAltitude <= 0.1) {
stopLanding()
return
}
val offsetThreshold = getOffsetThreshold(currentAltitude)
val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
println("自动调整 - 高度:%.2fm, x 偏移:%.2fm,y 偏移:%.2fm, 阈值:%.2fm, 下降速度:%.2fm/s".format(
currentAltitude,
currentXOffsetAbs,
currentYOffsetAbs,
offsetThreshold,
descentSpeed
))
val adjustParam = VirtualStickFlightControlParam().apply {
rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
verticalControlMode = VerticalControlMode.VELOCITY
yawControlMode = YawControlMode.ANGULAR_VELOCITY
rollPitchControlMode = RollPitchControlMode.ANGLE
if (currentXOffsetAbs > offsetThreshold) {
roll = if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
} else {
roll = 0.0
}
if (currentYOffsetAbs > offsetThreshold) {
pitch = if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
} else {
pitch = 0.0
}
yaw = 0.0
verticalThrottle = -descentSpeed
}
VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
virtualStickHandler?.postDelayed(this, 100)
}
}
virtualStickHandler?.post(adjustTask)
toastResult?.postValue(DJIToastResult.success("开始动态位置调整"))
}
private fun stopLanding() {
virtualStickHandler?.removeCallbacksAndMessages(null)
FlightControllerKey.KeyStartAutoLanding.create().action(
{
toastResult?.postValue(DJIToastResult.success("桨叶动力关闭"))
Log.i("stopLanding", "桨叶动力关闭成功")
},
{
toastResult?.postValue(DJIToastResult.failed("桨叶动力关闭失败"))
Log.i("stopLanding", "桨叶动力关闭失败!!")
}
)
cleanupVirtualStick()
toastResult?.postValue(DJIToastResult.success("降落完成"))
}
private fun getOffsetThreshold(altitude: Double): Double {
return when {
altitude > HIGH_ALTITUDE -> 1.0
altitude > MID_ALTITUDE -> 0.5
altitude > LOW_ALTITUDE -> 0.4
else -> 0.3
}
}
private fun getDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {
val threshold = getOffsetThreshold(altitude)
return when {
xOffset > threshold * 2 || yOffset > threshold * 2 -> 0.0
xOffset > threshold || yOffset > threshold -> 0.1
altitude > MID_ALTITUDE -> 0.5
altitude > LOW_ALTITUDE -> 0.2
else -> 0.1
}
}