27 lines
1.1 KiB
Python
27 lines
1.1 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)
|