def test_geodetic():
xyz = pm.geodetic2ecef(*lla0)
assert xyz == approx(xyz0)
assert pm.geodetic2ecef(*rlla0, deg=False) == approx(xyz)
with pytest.raises(ValueError):
pm.geodetic2ecef(lla0[0], lla0[1], -1)
with pytest.raises(ValueError):
pm.geodetic2ecef(-100, lla0[1], lla0[2])
with pytest.raises(ValueError):
pm.geodetic2ecef(lla0[0], -200, lla0[2])
assert pm.ecef2geodetic(*xyz) == approx(lla0)
assert pm.ecef2geodetic(*xyz, deg=False) == approx(rlla0)
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
with pytest.raises(ValueError):
pm.aer2geodetic(aer0[0], aer0[1], -1, *lla0)
assert lla2 == approx(lla1)
assert rlla2 == approx(rlla1)
assert pm.geodetic2aer(*lla2, *lla0) == approx(aer0)
assert pm.geodetic2aer(*rlla2, *rlla0, deg=False) == approx(raer0)
anan = np.empty((10, 10))
anan.fill(np.nan)
assert np.isnan(pm.geodetic2aer(anan, anan, anan, *lla0)).all()
assert np.isnan(pm.aer2geodetic(anan, anan, anan, *lla0)).all()
def test_aer():
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
with pytest.raises(ValueError):
pm.aer2geodetic(aer0[0], aer0[1], -1, *lla0)
assert lla2 == approx(lla1)
assert rlla2 == approx(rlla1)
assert pm.geodetic2aer(*lla2, *lla0) == approx(aer0)
assert pm.geodetic2aer(*rlla2, *rlla0, deg=False) == approx(raer0)
def test_aer_geodetic(aer, lla, lla0):
assert pm.aer2geodetic(*aer, *lla0) == approx(lla)
raer = (radians(aer[0]), radians(aer[1]), aer[2])
rlla0 = (radians(lla0[0]), radians(lla0[1]), lla0[2])
assert pm.aer2geodetic(*raer, *rlla0, deg=False) == approx((radians(lla[0]), radians(lla[1]), lla[2]))
with pytest.raises(ValueError):
pm.aer2geodetic(aer[0], aer[1], -1, *lla0)
assert pm.geodetic2aer(*lla, *lla0) == approx(aer, rel=1e-3)
assert pm.geodetic2aer(radians(lla[0]), radians(lla[1]), lla[2], *rlla0, deg=False) == approx(raer, rel=1e-3)
def test_aer():
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
with pytest.raises(ValueError):
pm.aer2geodetic(aer0[0], aer0[1], -1, *lla0)
assert lla2 == approx(lla1)
assert rlla2 == approx(rlla1)
assert pm.geodetic2aer(*lla2, *lla0) == approx(aer0)
assert pm.geodetic2aer(*rlla2, *rlla0, deg=False) == approx(raer0)
def getimgind(imgs: xarray.Dataset, lla: np.ndarray, az: np.ndarray, el: np.ndarray) -> np.ndarray:
""" find pixels in images according to lat,lon,alt or az,el spec"""
if lla is not None:
lla = np.atleast_2d(lla)
assert lla.ndim == 2
assert lla.shape[1] == 3
N = lla.shape[0]
elif az is not None and el is not None:
az = np.atleast_1d(az)
el = np.atleast_1d(el)
assert az.ndim == 1 and el.ndim == 1
N = az.size
else:
raise ValueError("must specify LLA or az/el")
# %% work
ind = np.empty((N, 2), dtype=int)
if lla is not None:
az, el, _ = pm.geodetic2aer(lla[:, 0], lla[:, 1], lla[:, 2] * 1000, imgs.lat, imgs.lon, imgs.alt_m)
for i, (a, e) in enumerate(zip(az, el)):
ind[i, :] = fnd.findClosestAzel(imgs.az, imgs.el, a, e)
ind = np.unique(ind, axis=0)
assert ind.ndim == 2
assert ind.shape[1] == 2 # row, column
return ind
def calc_distance(lerp_data, latitude, longitude, altitude):
for dat in lerp_data.items():
for d in dat[1]:
az, el, dst = pm.geodetic2aer(dat[1][d]['lat'], dat[1][d]['lng'],
0, latitude, longitude, altitude)
lerp_data[dat[0]][d].update({'az': az, 'el': el, 'range': dst})
return lerp_data
def test_geodetic():
assert_allclose(pm.ecef2geodetic(tx, ty, tz), (ec2la, ec2lo, ec2a),
rtol=0.01,
err_msg='ecef2geodetic: ' +
str(pm.ecef2geodetic(tx, ty, tz)))
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(g2az, g2el, g2rn),
rtol=0.05,
err_msg='geodetic2aer: {}'.format(
pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt)))
assert_allclose(pm.geodetic2ecef(tlat, tlon, talt), (g2x, g2y, g2z),
rtol=0.01,
err_msg='geodetic2ecef: {}'.format(
pm.geodetic2ecef(tlat, tlon, talt)))
def look_angles(self, position):
"""Returns (az, el, range) from antenna to UAV in degrees and meters."""
return pymap3d.geodetic2aer(
position.latitude,
position.longitude,
position.altitude_m,
self.antenna_latitude or 0,
self.antenna_longitude or 0,
self.antenna_altitude_m or 0,
ellipsoid,
True # degrees (instead of radians)
)
def test_aer_geodetic(aer, lla, lla0):
lat1, lon1, alt1 = pm.aer2geodetic(*aer, *lla0)
assert lat1 == approx(lla[0])
assert lon1 == approx(lla[1])
assert alt1 == approx(lla[2])
assert isinstance(lat1, float)
assert isinstance(lon1, float)
assert isinstance(alt1, float)
raer = (radians(aer[0]), radians(aer[1]), aer[2])
rlla0 = (radians(lla0[0]), radians(lla0[1]), lla0[2])
assert pm.aer2geodetic(*raer, *rlla0, deg=False) == approx(
(radians(lla[0]), radians(lla[1]), lla[2]))
with pytest.raises(ValueError):
pm.aer2geodetic(aer[0], aer[1], -1, *lla0)
assert pm.geodetic2aer(*lla, *lla0) == approx(aer, rel=1e-3)
assert pm.geodetic2aer(radians(lla[0]),
radians(lla[1]),
lla[2],
*rlla0,
deg=False) == approx(raer, rel=1e-3)
def get_hadec(pred_pos, log_data):
''' Convert predicted position from Geodetic to HADEC '''
# Get AZEL and HADEC if data is updated
if is_pos_updated(pred_pos[0:3], lite.null_pos[0:3]):
az, el, range = pm.geodetic2aer(pred_pos[0], pred_pos[1], pred_pos[2],
lite.ref_pos[0], lite.ref_pos[1], lite.ref_pos[2])
[ha, dec] = azel_to_hadec([az, el, range])
ha += lite.offset_ha
dec += lite.offset_dec
# Set AZEL and HADEC to dummy values if no data update
else:
az = el = range = ha = dec = -404
# Push data to lite.log[]
log_data['azel'] = [az, el, range]
log_data['hadec'] = [ha, dec]
log_data['hadec_offset'] = [lite.offset_ha, lite.offset_dec]
return [ha, dec]
def calcGeodesics(df, lat0, lon0, alt0):
latn = df.loc[:, ' lat']
lonn = df.loc[:, ' lon']
altn = df.loc[:, ' alt']
azn = []
eln = []
dstn = []
for i in range(len(latn)):
az, el, dst = pm.geodetic2aer(latn.iat[i], lonn.iat[i], altn.iat[i],
lat0, lon0, alt0)
azn.append(az)
eln.append(el)
dstn.append(dst)
df['azimuth'] = azn
df['elevation'] = eln
df['slantrange'] = dstn
return df
read_name = 'state.txt'
write_name = 'dst_state.csv'
rf = open(read_name, 'r')
reader = csv.reader(rf)
header = next(reader)
header.append('az')
header.append('el')
header.append('dst')
data = [header]
for row in reader:
lat = float(row[7])
lon = float(row[8])
alt = float(row[9]) + offset_alt
az, el, dst = pm.geodetic2aer(lat, lon, alt, obs_geo['lat'],
obs_geo['lon'], obs_geo['alt'])
row.append(az)
row.append(el)
row.append(dst)
data.append(row)
rf.close()
wf = open(write_name, 'w')
writer = csv.writer(wf)
writer.writerows(data)
wf.close()
def test_allnan():
np = pytest.importorskip("numpy")
anan = np.empty((10, 10))
anan.fill(nan)
assert np.isnan(pm.geodetic2aer(anan, anan, anan, *lla0)).all()
assert np.isnan(pm.aer2geodetic(anan, anan, anan, *lla0)).all()
def test_allnan():
anan = np.empty((10, 10))
anan.fill(nan)
assert np.isnan(pm.geodetic2aer(anan, anan, anan, *lla0)).all()
assert np.isnan(pm.aer2geodetic(anan, anan, anan, *lla0)).all()
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt)
assert_allclose(
pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False),
(x1, y1, z1))
assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef')
assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt),
err_msg='ecef2geodetic')
if pyproj:
assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt),
(x1, y1, z1))
assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1),
(tlon, tlat, talt))
lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1),
err_msg='aer2geodetic')
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='geodetic2aer')
x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose(
pm.aer2ecef(radians(taz),
radians(tel),
tsrange,
radians(tlat),
radians(tlon),
talt,
deg=False), (a2x, a2y, a2z))
assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef')
assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='ecef2aer')
e1, n1, u1 = pm.aer2enu(taz, tel, tsrange)
assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu')
assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0),
err_msg='aer2ned')
assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange),
err_msg='enu2aer')
assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange),
err_msg='ned2aer')
assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='enu2ecef')
assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt),
(e1, n1, u1),
err_msg='ecef2enu')
assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt),
(n1, e1, -u1),
err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
#%%
e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt)
assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt),
(n3, e3, -u3))
assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='ned2geodetic')
def test_scalar_nan():
a, e, r = pm.geodetic2aer(nan, nan, nan, *lla0)
assert isnan(a) and isnan(e) and isnan(r)
lat, lon, alt = pm.aer2geodetic(nan, nan, nan, *lla0)
assert isnan(lat) and isnan(lon) and isnan(alt)
alt_input = 300
while len(ad) > i:
vehicle = connect(ad[i], wait_ready=True, timeout=60)
vehicle_list.append(vehicle)
alt_goal = vehicle_list[i].location.global_frame.alt
lat_home = vehicle_list[i].location.global_frame.lat
lon_home = vehicle_list[i].location.global_frame.lon
alt_home = vehicle_list[i].location.global_frame.alt
# input goal location
az, el, range = pm.geodetic2aer(lat_goal, lon_goal, alt_goal, lat_home,
lon_home, alt_home)
print(ad[i])
print("home to goal : 方位角 = %.1f [deg], 仰角 = %.1f [deg], 直線距離 = %.1f [m]" %
(az, el, range))
distances[i] = range
#vehicle.close
#del vehicle
time.sleep(2)
i = i + 1
index_min = np.argmin(distances)
print("The nearest drone is :", ad[index_min])
print(vehicle_list)
###----------------------------- 3. write mission-------------------------------------------------
def test_allnan():
anan = np.empty((10, 10))
anan.fill(nan)
assert np.isnan(pm.geodetic2aer(anan, anan, anan, *lla0)).all()
assert np.isnan(pm.aer2geodetic(anan, anan, anan, *lla0)).all()
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
xyz1 = pm.geodetic2ecef(*lla0)
assert_allclose(pm.geodetic2ecef(*rlla0, deg=False),
xyz1,
err_msg='geodetic2ecef: rad')
assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg')
assert_allclose(pm.ecef2geodetic(*xyz1),
lla0,
err_msg='ecef2geodetic: deg')
assert_allclose(pm.ecef2geodetic(*xyz1, deg=False),
rlla0,
err_msg='ecef2geodetic: rad')
if pyproj:
assert_allclose(
pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1)
assert_allclose(pyproj.transform(ecef, lla, *xyz1),
(lla0[1], lla0[0], lla0[2]))
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg')
assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad')
assert_allclose(pm.geodetic2aer(*lla2, *lla0),
aer0,
err_msg='geodetic2aer: deg')
assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False),
raer0,
err_msg='geodetic2aer: rad')
# %% aer-ecef
xyz2 = pm.aer2ecef(*aer0, *lla0)
assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False),
axyz0,
err_msg='aer2ecef:rad')
assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg')
assert_allclose(pm.ecef2aer(*xyz2, *lla0),
aer0,
err_msg='ecef2aer:deg')
assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False),
raer0,
err_msg='ecef2aer:rad')
# %% aer-enu
enu1 = pm.aer2enu(*aer0)
ned1 = (enu1[1], enu1[0], -enu1[2])
assert_allclose(enu1, enu0, err_msg='aer2enu: deg')
assert_allclose(pm.aer2enu(*raer0, deg=False),
enu0,
err_msg='aer2enu: rad')
assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned')
assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg')
assert_allclose(pm.enu2aer(*enu1, deg=False),
raer0,
err_msg='enu2aer: rad')
assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer')
# %% enu-ecef
assert_allclose(pm.enu2ecef(*enu1, *lla0),
xyz2,
err_msg='enu2ecef: deg')
assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False),
xyz2,
err_msg='enu2ecef: rad')
assert_allclose(pm.ecef2enu(*xyz2, *lla0),
enu1,
err_msg='ecef2enu:deg')
assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False),
enu1,
err_msg='ecef2enu:rad')
assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla2, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert_allclose(pm.geodetic2ned(*lla2, *lla0),
ned3,
err_msg='geodetic2ned: deg')
assert_allclose(pm.enu2geodetic(*enu3, *lla0),
lla2,
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(*ned3, *lla0),
lla2,
err_msg='ned2geodetic')
def calculate_air_distance_meters(origin, target):
alt = origin['alt']
aer = pymap3d.geodetic2aer(origin['lat'], origin['lon'], alt,
target['lat'], target['lon'], alt)
distance = aer[2]
return distance
def calculate_aer(origin, target):
aer = pymap3d.geodetic2aer(target['lat'], target['lon'], target['alt'],
origin['lat'], origin['lon'], origin['alt'])
return { 'az': aer[0], 'el': aer[1], 'srange': aer[2] }
