SatMapsUseful

satellite_maps.py

from pymavlink import mavextra
import math
import numpy as np
import typing as typ

# a threshold that indicates that latitude between the two image centers
# is significantly different
LATITUDE_CHANGE_THRESHOLD = 1e-4

# for the reference geoid used by OpenStreetMap
EQUATORIAL_CIRCUMFERENCE_OF_EARTH = 40075016.686  # meters

# https://stackoverflow.com/questions/7477003/calculating-new-longitude-latitude-from-old-n-meters
# number of km per degree = ~111km (111.32 in google maps, but range varies
# between 110.567km at the equator and 111.699km at the poles)
METERS_PER_LATITUDE_DEGREE = 111111

MAP_TILE_SIZE = 256

RADIUS_OF_EARTH = 6378100  # meters

def longitude_scale(latitude: float):
    """Transfered from ArduPilot's Location::longitude_scale"""
    return max(np.cos(latitude), 0.01)


def diff_longitude(lon1: float, lon2: float):
    """ Transfered from ArduPilot's Location::diff_longitude
    """
    # check sign of longitudes (0x80000000 = 1 << 31)
    # lon & 1 << 31
    if np.sign(lon1) == np.sign(lon2):
        return lon1 - lon2
    return mavextra.angle_diff(lon1, lon2)


def get_distance_ned(lat1: float, lon1: float, lat2: float, lon2: float):
    """ Transfered from ArduPilot's Location::get_distance_ned
    """
    dlat = lat2 - lat1
    dlon = diff_longitude(lon2, lon1)
    north_m = np.radians(dlat) * RADIUS_OF_EARTH
    east_m = np.radians(dlon) * RADIUS_OF_EARTH * longitude_scale(np.radians((lat1 + lat2) / 2.0))
    return east_m, north_m
  
def apply_translation_in_meters_to_geo_coords(
    latitude: float,
    longitude: float,
    d_east_meters: float,
    d_north_meters: float,
) -> typ.Tuple[float, float]:
    """
    Returns:
        latitude, longitude
    """
    return mavextra.gps_offset(latitude, longitude, d_east_meters, d_north_meters)