40 lines
1.2 KiB
Python
40 lines
1.2 KiB
Python
import math
|
|
from target_simulator.core.models import Waypoint, Target, ManeuverType
|
|
from itertools import islice
|
|
|
|
w1 = Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=10,
|
|
target_azimuth_deg=10,
|
|
target_altitude_ft=1000,
|
|
)
|
|
w2 = Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=20,
|
|
target_azimuth_deg=45,
|
|
target_altitude_ft=1000,
|
|
)
|
|
path, _ = Target.generate_path_from_waypoints([w1, w2], use_spline=False)
|
|
|
|
print("path_len", len(path))
|
|
for p in list(islice(path, 5)):
|
|
t, x, y, z = p
|
|
tp_xy = math.atan2(x, y)
|
|
tp_old = -math.atan2(x, y)
|
|
az_cur = -math.degrees(math.atan2(x, y))
|
|
tp_from_az = math.radians(-az_cur)
|
|
r_nm = math.hypot(x, y) / 6076.12
|
|
print(
|
|
"time=%.2f x=%.2f y=%.2f r_nm=%.3f tp_xy=%.6f tp_old=%.6f az_cur=%.6f tp_from_az=%.6f"
|
|
% (t, x, y, r_nm, tp_xy, tp_old, az_cur, tp_from_az)
|
|
)
|
|
|
|
# Check waypoint azimuths -> plotted theta
|
|
for wp in [w1, w2]:
|
|
wp_az_rad = math.radians(wp.target_azimuth_deg or 0.0)
|
|
print("wp az deg=%.1f az_rad=%.6f" % (wp.target_azimuth_deg, wp_az_rad))
|
|
|
|
start_wp = w1
|
|
start_theta = math.radians(start_wp.target_azimuth_deg or 0.0)
|
|
print("start_theta_from_wp=%.6f" % start_theta)
|