diff --git a/scenarios.json b/scenarios.json index 9e26dfe..5554f54 100644 --- a/scenarios.json +++ b/scenarios.json @@ -1 +1,126 @@ -{} \ No newline at end of file +{ + "scenario1": { + "name": "scenario1", + "targets": [ + { + "target_id": 0, + "active": true, + "traceable": true, + "trajectory": [ + { + "maneuver_type": "Fly to Point", + "duration_s": 10.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 20.0, + "target_azimuth_deg": 0.0 + }, + { + "maneuver_type": "Fly for Duration", + "target_velocity_fps": 1670.9318999999994, + "target_heading_deg": 10.0, + "duration_s": 300.0, + "target_altitude_ft": 10000.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 400.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 25.0, + "target_azimuth_deg": -20.0 + } + ] + } + ] + }, + "scenario2": { + "name": "scenario2", + "targets": [ + { + "target_id": 0, + "active": true, + "traceable": true, + "trajectory": [ + { + "maneuver_type": "Fly to Point", + "duration_s": 10.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 10.0, + "target_azimuth_deg": 1.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 200.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 20.0, + "target_azimuth_deg": 10.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 200.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 30.0, + "target_azimuth_deg": -10.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 200.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 35.0, + "target_azimuth_deg": 10.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 200.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 35.0, + "target_azimuth_deg": 30.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 200.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 20.0, + "target_azimuth_deg": 45.0 + } + ], + "use_spline": true + }, + { + "target_id": 1, + "active": true, + "traceable": true, + "trajectory": [ + { + "maneuver_type": "Fly to Point", + "duration_s": 10.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 10.0, + "target_azimuth_deg": 10.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 10.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 20.0, + "target_azimuth_deg": 20.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 30.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 30.0, + "target_azimuth_deg": 30.0 + }, + { + "maneuver_type": "Fly to Point", + "duration_s": 30.0, + "target_altitude_ft": 10000.0, + "target_range_nm": 35.0, + "target_azimuth_deg": -10.0 + } + ], + "use_spline": false + } + ] + } +} \ No newline at end of file diff --git a/settings.json b/settings.json index 265b730..fd7a685 100644 --- a/settings.json +++ b/settings.json @@ -3,7 +3,7 @@ "scan_limit": 60, "max_range": 100, "geometry": "1599x1024+481+287", - "last_selected_scenario": null, + "last_selected_scenario": "scenario2", "connection": { "target": { "type": "sfp", diff --git a/target_simulator/gui/ppi_display.py b/target_simulator/gui/ppi_display.py index f0bc515..1d07ed7 100644 --- a/target_simulator/gui/ppi_display.py +++ b/target_simulator/gui/ppi_display.py @@ -250,7 +250,10 @@ class PPIDisplay(ttk.Frame): for point in path: x_ft, y_ft = point[1], point[2] r_ft = math.sqrt(x_ft**2 + y_ft**2) - az_rad_plot = -math.atan2(x_ft, y_ft) + # Use the same plotting convention used elsewhere: theta_plot = atan2(x, y) + # (update_targets computes theta via -current_azimuth_deg where + # current_azimuth_deg = -degrees(atan2(x,y)), which yields atan2(x,y)). + az_rad_plot = math.atan2(x_ft, y_ft) path_rs.append(r_ft / NM_TO_FT) path_thetas.append(az_rad_plot) self._path_plot.set_data(path_thetas, path_rs) @@ -258,13 +261,17 @@ class PPIDisplay(ttk.Frame): for wp in waypoints: if wp.maneuver_type == ManeuverType.FLY_TO_POINT: r_nm = wp.target_range_nm or 0.0 - az_rad_plot = np.deg2rad(-(wp.target_azimuth_deg or 0.0)) + # The path uses theta_plot = atan2(x, y). Waypoint azimuths + # provided in the waypoint are geometric azimuth degrees + # (0 = North, positive CCW). Convert directly to radians so + # plotted waypoint markers align with the generated path. + az_rad_plot = np.deg2rad(wp.target_azimuth_deg or 0.0) wp_rs.append(r_nm) wp_thetas.append(az_rad_plot) self._waypoints_plot.set_data(wp_thetas, wp_rs) start_wp = waypoints[0] start_r = start_wp.target_range_nm or 0.0 - start_theta = np.deg2rad(-(start_wp.target_azimuth_deg or 0.0)) + start_theta = np.deg2rad(start_wp.target_azimuth_deg or 0.0) self._start_plot.set_data([start_theta], [start_r]) if self.canvas: self.canvas.draw() diff --git a/tools/trajectory_test.py b/tools/trajectory_test.py new file mode 100644 index 0000000..9fc26c3 --- /dev/null +++ b/tools/trajectory_test.py @@ -0,0 +1,26 @@ +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)