Check if goto_location has reached target position

MAVSDK
1

This surely has been asked before and I just cannot find it. I can’t be the first one.

How to check if a drone has reached a target position after issuing a goto_location command?

Right now, my solution is to regularly check the drone’s position and consider the waypoint done when the difference between actual and target position lies within a certain tolerance. It then waits a few seconds to let the drone settle itself at the new position and moves on.

    import math
    from mavsdk import System

    TOLERANCE_DEG = 0.00005
    TOLERANCE_M = 0.3
    WAIT_FOR_TARGET = 2
    uav = System()
    lat = ...
    lon = ...
    alt = ...
    yaw = ...
    ...
    await uav.action.goto_location(lat, lon, alt, yaw)
    
    last = 0
    async for position in uav.telemetry.position():
        curr = time.time()
        if (curr - last >= 1):
            last = curr
            diff_lat = math.fabs(position.latitude_deg - lat) - TOLERANCE_DEG
            diff_lon = math.fabs(position.longitude_deg - lon) - TOLERANCE_DEG
            diff_alt = math.fabs(position.relative_altitude_m - alt) - TOLERANCE_M
            if (diff_lat <= 0 and diff_lon <= 0 and diff_alt <= 0):
                print("Reached target vincinity, wait another " + str(WAIT_FOR_TARGET) + " s")
                asyncio.sleep(WAIT_FOR_TARGET)
                print("Reached target position")
                break

However, this feels a little hacky. This approach is based on a lot of estimates that may change with drone model or situation.

Is there a better solution to do this?

Thanks and best regards.

2

Hello, I had the same problem. I needed to execute some custom code when I reached a checkpoint of a mission. I implemented something similar to your approach because I don’t think there is a possibility to get a callback when the point is reached. For missions, you can get a callback when the drone leaves, but that is also not super helpful.

Here is my solution. It checks whether the distance in meter is lower than the acceptance distance:

val missionPlan = MissionPlan(flightPlan.checkpoints?.map {
    Mission.MissionItem(
        it.latitude,
        it.longitude,
        10f,
        5f,
        false,
        0f,
        0f,
        Mission.MissionItem.CameraAction.TAKE_PHOTO,
        5f,
        0.0,
        1f,
        0f,
        0f,
        Mission.MissionItem.VehicleAction.NONE
    )
} ?: emptyList())
drone?.mission?.uploadMission(missionPlan)?.blockingAwait()
drone?.mission?.setReturnToLaunchAfterMission(true)?.blockingAwait()
drone?.mission?.setCurrentMissionItem(0)?.blockingAwait()
drone?.mission?.startMission()?.blockingAwait()

var currentCheckpoint = missionPlan.missionItems[0]
var checkpointReached = false
drone?.mission?.missionProgress?.subscribe {
    currentCheckpoint = missionPlan.missionItems[it.current]
    checkpointReached = false
}
drone?.telemetry?.position?.subscribe {
    if (checkpointReached) {
        return@subscribe
    }
    val distanceM = haversine(
        currentCheckpoint.latitudeDeg,
        currentCheckpoint.longitudeDeg,
        it.latitudeDeg,
        it.longitudeDeg
    )
    if (distanceM < 1) {
        checkpointReached = true
        println("Checkpoint reached!")
    }
}

Distance function:

private fun haversine(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double {
    val radiusEarth = 6371000
    val dLat = Math.toRadians(lat2 - lat1)
    val dLon = Math.toRadians(lon2 - lon1)
    val originLat = Math.toRadians(lat1)
    val destinationLat = Math.toRadians(lat2)

    val a = sin(dLat / 2).pow(2) + sin(dLon / 2).pow(2) * cos(originLat) * cos(destinationLat)
    val c = 2 * atan2(sqrt(a), sqrt(1 - a))
    return radiusEarth * c
}

The same approach can be used for goto_location. Maybe this helps someone in the future.