goto_location function

Hello

I’m in my third bachelor engineering technology. For my bachelorsassignment we have to write code for an autonomous flight test. We’re supposed to write a mission using some specific functions. One of the functions we miss is a ‘goto_location’ function. Right now I’m trying to convert the distance in degree’s between two coördinates into a distance in meters but I still get some errors. I am using following code: ` import math “”“radius of the Earth”“” r = 6373.0 class Waypoint(): def init(self, lat, lon): self.lat = lat self.lon = lon

async def distancelat(self, wp1, wp2):
    """LATITUDE"""
    self.wp1 = wp1
    self.wp2 = wp2
    lat1rad = math.radians(wp1.lat)
    lat2rad = math.radians(wp2.lat)

    # change in coordinates
    dlon = 0
    dlat = lat2rad - lat1rad
    # Haversine formula
    a1 = math.sin(dlat / 2) ** 2 + math.cos(lat1rad) * math.cos(lat2rad) * math.sin(dlon / 2) ** 2
    c1 = 2 * math.atan2(math.sqrt(a1), math.sqrt(1 - a1))
    distancelat = float(r * c1*1000)
    return distancelat

async def distancelon(self, wp1, wp2):
    """LONGITUDE"""
    self.wp1 = wp1
    self.wp2 = wp2
    lon1rad = math.radians(wp1.lon)
    lon2rad = math.radians(wp2.lon)
    lat1rad = math.radians(wp1.lat)
    lat2rad = math.radians(wp2.lat)

    # change in coordinates
    dlon = lon2rad - lon1rad
    dlat = 0
    # Haversine formula
    a = math.sin(dlat / 2) ** 2 + math.cos(lat1rad) * math.cos(lat2rad) * math.sin(dlon / 2) ** 2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    distancelong = float(r * c*1000)
    return distancelong

`

By using using the offboard function ‘set_position_ned’ I try to make the drone fly to it’s new location.

wp1 = Waypoint(47.3977395, 8.5455929) wp2 = Waypoint(47.398039859999997, 8.5455725400000002) mnorth = Waypoint.distancelat(wp1, wp2) meast = Waypoint.distancelon(wp1, wp2) await drone.offboard.set_position_ned( (PositionNedYaw(mnorth, meast, -5, 0)))

When trying to complete the mission I get the errors ‘no RC and no offboard’. Another error I get is that parameter ‘wp2’ is ‘‘unfilled’’.

Is there any way i can solve this? Is there an easier option?

Hope to hear from you soon and thanks in advance Katrijn Chys

[julianoes]

Yes 😄

[JonasVautherin]

You mean this?

def global_from_local(north_m, east_m, ref_lat, ref_lon):
    world_radius_m = 6371000
    ref_lat_rad = math.pi / 180 * ref_lat
    ref_lon_rad = math.pi / 180 * ref_lon
    x_rad = north_m / world_radius_m
    y_rad = east_m / world_radius_m
​
    c = math.sqrt(x_rad * x_rad + y_rad * y_rad);
​
    if (c == 0):
        return ref_lat, ref_lon
​
    sin_c = math.sin(c)
    cos_c = math.cos(c)
​
    ref_sin_lat = math.sin(ref_lat_rad)
    ref_cos_lat = math.cos(ref_lat_rad)
​
    lat_rad = math.asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c)
    lon_rad = (ref_lon_rad + math.atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c))
​
    global_lat = 180 / math.pi * lat_rad
    global_lon = 180 / math.pi * lon_rad
​
    return global_lat, global_lon