fix first waypoint into preview
add dinamic movement for target
This commit is contained in:
VALLONGOL
parent
e62b584981
commit
b20bd40b37
@ -1,101 +1,100 @@
|
||||
x_nm,y_nm
|
||||
0.17452406437283513,9.998476951563912
|
||||
0.2859422147817867,10.279638104623308
|
||||
0.4421393353133215,10.609933583832394
|
||||
0.636432547474389,10.985187585712282
|
||||
0.8621389727719386,11.401224306784082
|
||||
1.1125757327129193,11.853867943568899
|
||||
1.3810599488042814,12.338942692587844
|
||||
1.6609087425529734,12.852272750362022
|
||||
1.9454392354659444,13.389682313412546
|
||||
2.227968549050145,13.946995578260525
|
||||
2.5018138048125236,14.520036741427065
|
||||
2.7602921242600305,15.104629999433275
|
||||
2.996720628899614,15.696599548800267
|
||||
3.2044164402382243,16.291769586049142
|
||||
3.3766966797828104,16.885964307701016
|
||||
3.5068784690403216,17.475007910277
|
||||
3.588278929517707,18.054724590298196
|
||||
3.6142151827219164,18.620938544285714
|
||||
3.578004350159902,19.169473968760666
|
||||
3.472963553338606,19.696155060244156
|
||||
3.4729635533386065,19.69615506024416
|
||||
3.272762644392875,20.217383756990923
|
||||
2.964843068011936,20.750866222507394
|
||||
2.5631793345343286,21.294382991192247
|
||||
2.081745954298589,21.845714597444157
|
||||
1.5345174376432555,22.402641575661793
|
||||
0.9354682949068648,22.962944460243826
|
||||
0.2985730364279542,23.52440378558893
|
||||
-0.36219382745493856,24.084800086095772
|
||||
-1.032857786403277,24.641913896163032
|
||||
-1.699444330078522,25.193525750189377
|
||||
-2.347978948142139,25.737416182573483
|
||||
-2.964487130255587,26.271365727714013
|
||||
-3.5349943660803307,26.793154920009652
|
||||
-4.045526145277833,27.30056429385906
|
||||
-4.482107957509558,27.79137438366092
|
||||
-4.830765292436963,28.263365723813894
|
||||
-5.077523639721516,28.714318848716662
|
||||
-5.208408489024677,29.14201429276789
|
||||
-5.2094453300079095,29.544232590366253
|
||||
-5.2094453300079095,29.544232590366242
|
||||
-5.087215806911271,29.93160758132983
|
||||
-4.8634013589893685,30.31504050771131
|
||||
-4.54667716842549,30.69271300726126
|
||||
-4.145718417402923,31.062806717730222
|
||||
-3.6692002881049564,31.42350327686877
|
||||
-3.1257979627148766,31.772984322427458
|
||||
-2.5241866234159707,32.10943149215684
|
||||
-1.873041452391528,32.43102642380748
|
||||
-1.1810376318248348,32.735950755129934
|
||||
-0.4568503438991798,33.02238612387475
|
||||
0.29084522920215017,33.28851416779252
|
||||
1.0533739052958677,33.53251652463377
|
||||
1.822060502198683,33.75257483214907
|
||||
2.5882298377273103,33.94687072808898
|
||||
3.343206729698462,34.113585850204075
|
||||
4.078315995928851,34.25090183624488
|
||||
4.784882454235189,34.35700032396197
|
||||
5.454230922434187,34.430062951105924
|
||||
6.077686218342558,34.46827135542727
|
||||
6.077686218342562,34.468271355427284
|
||||
6.6950615437258465,34.47971290071672
|
||||
7.3476044195818755,34.472832854810775
|
||||
8.028791142575807,34.44634971152879
|
||||
8.732098009372796,34.3989819646901
|
||||
9.451001316637997,34.32944810811404
|
||||
10.178977361036566,34.23646663561996
|
||||
10.909502439233654,34.11875604102719
|
||||
11.636052847894423,33.97503481815507
|
||||
12.352104883684024,33.80402146082294
|
||||
13.051134843267612,33.60443446285013
|
||||
13.726619023310347,33.37499231805599
|
||||
14.372033720477376,33.114413520259845
|
||||
14.98085523143386,32.82141656328105
|
||||
15.546559852844958,32.49471994093893
|
||||
16.062623881375814,32.13304214705283
|
||||
16.522523613691593,31.735101675442085
|
||||
16.919735346457447,31.29961701992603
|
||||
17.247735376338525,30.825306674324008
|
||||
17.499999999999996,30.310889132455358
|
||||
17.499999999999996,30.310889132455355
|
||||
17.667951456825268,29.722378176057024
|
||||
17.752649951768674,29.034858901953363
|
||||
17.76202872985173,28.26065677659477
|
||||
17.704021036095945,27.412097266431687
|
||||
17.586560115522854,26.501505837914515
|
||||
17.417579213153953,25.541207957493693
|
||||
17.205011574010783,24.54352909161962
|
||||
16.956790443114837,23.52079470674273
|
||||
16.68084906548765,22.485330269313444
|
||||
16.385120686150735,21.449461245782175
|
||||
16.0775385501256,20.425513102599343
|
||||
15.766035902433776,19.42581130621538
|
||||
15.45854598809677,18.462681323080695
|
||||
15.1630020521361,17.5484486196457
|
||||
14.88733733957329,16.69543866236083
|
||||
14.639485095429851,15.9159769176765
|
||||
14.427378564727302,15.222388852043133
|
||||
14.258950992487165,14.626999931911136
|
||||
14.142135623730947,14.142135623730947
|
||||
1.7364817766693033,9.84807753012208
|
||||
1.8196915585801245,9.997286852043835
|
||||
1.909698355298528,10.164705993254746
|
||||
2.006314644389864,10.349354914685591
|
||||
2.1093529034194827,10.550253577267155
|
||||
2.218625609952734,10.766421941930222
|
||||
2.3339452415549684,10.99687996960557
|
||||
2.4551242757915364,11.240647621223985
|
||||
2.581975190227787,11.496744857716246
|
||||
2.714310462429072,11.764191640013136
|
||||
2.851942569960741,12.04200792904544
|
||||
2.9946839903881437,12.329213685743936
|
||||
3.14234720127663,12.624828871039403
|
||||
3.2947446801915516,12.927873445862636
|
||||
3.4516889046982575,13.237367371144403
|
||||
3.612992352362098,13.552330607815493
|
||||
3.7784675007484245,13.871783116806688
|
||||
3.947926827422586,14.19474485904877
|
||||
4.121182809949932,14.520235795472518
|
||||
4.298047925895815,14.847275887008719
|
||||
4.478334652825583,15.17488509458815
|
||||
4.661855468304586,15.502083379141595
|
||||
4.848422849898178,15.827890701599843
|
||||
5.037849275171705,16.151327022893664
|
||||
5.2299472216905185,16.471412303953848
|
||||
5.424529167019969,16.787166505711173
|
||||
5.621407588725407,17.09760958909643
|
||||
5.8203949643721815,17.401761515040384
|
||||
6.021303771525644,17.698642244473834
|
||||
6.223946487751144,17.987271738327554
|
||||
6.4281355906140325,18.26666995753233
|
||||
6.6336835576796584,18.535856863018942
|
||||
6.840402866513372,18.79385241571817
|
||||
6.840402866513374,18.79385241571817
|
||||
7.064412663195302,19.043619595904882
|
||||
7.319986102939524,19.28896147814137
|
||||
7.604166677012322,19.530158164789555
|
||||
7.913997876679983,19.767489758211376
|
||||
8.246523193208787,20.001236360768743
|
||||
8.598786117865023,20.231678074823588
|
||||
8.96783014191497,20.459095002737843
|
||||
9.350698756624908,20.683767246873423
|
||||
9.744435453261131,20.90597490959226
|
||||
10.146083723089918,21.12599809325628
|
||||
10.552687057377549,21.3441169002274
|
||||
10.961288947390312,21.560611432867567
|
||||
11.368932884394491,21.775761793538678
|
||||
11.772662359656369,21.989848084602684
|
||||
12.169520864442228,22.203150408421493
|
||||
12.556551890018351,22.41594886735704
|
||||
12.930798927651024,22.62852356377125
|
||||
13.289305468606532,22.84115460002604
|
||||
13.629115004151155,23.054122078483353
|
||||
13.947271025551181,23.267706101505095
|
||||
14.24081702407289,23.482186771453204
|
||||
14.506796490982568,23.6978441906896
|
||||
14.742252917546494,23.91495846157622
|
||||
14.944229795030958,24.133809686474972
|
||||
15.109770614702246,24.354677967747794
|
||||
15.23591886782663,24.57784340775661
|
||||
15.319718045670406,24.80358610886334
|
||||
15.358211639499848,25.032186173429917
|
||||
15.348443140581244,25.26392370381826
|
||||
15.28745604018088,25.499078802390304
|
||||
15.172293829565035,25.737931571507964
|
||||
15.0,25.980762113533167
|
||||
14.999999999999998,25.98076211353316
|
||||
14.76007873005941,26.230939963143115
|
||||
14.447076186168827,26.491050501357915
|
||||
14.065598841372388,26.760197600053854
|
||||
13.62025316871424,27.037485131107207
|
||||
13.11564564123853,27.322016966394273
|
||||
12.556382731989396,27.612896977791323
|
||||
11.947070914010986,27.909229037174644
|
||||
11.292316660347446,28.210117016420522
|
||||
10.596726444042913,28.514664787405238
|
||||
9.86490673814154,28.82197622200508
|
||||
9.101464015687464,29.13115519209633
|
||||
8.311004749724834,29.441305569555276
|
||||
7.498135413297791,29.751531226258194
|
||||
6.66746247945048,30.060936034081376
|
||||
5.823592421227048,30.368623864901103
|
||||
4.971131711671633,30.67369859059366
|
||||
4.114686823828384,30.97526408303533
|
||||
3.2588642307414455,31.272424214102394
|
||||
2.4082704054549575,31.564282855671145
|
||||
1.5675118210130687,31.84994387961786
|
||||
0.7411949504599216,32.12851115781883
|
||||
-0.06607373316034426,32.39908856215034
|
||||
-0.8496877568035757,32.660779964488654
|
||||
-1.6050406474256338,32.91268923671007
|
||||
-2.3275259319823753,33.15392025069089
|
||||
-3.0125371374296517,33.38357687830738
|
||||
-3.6554677907233284,33.600762991435815
|
||||
-4.2517114188192515,33.80458246195249
|
||||
-4.796661548673278,33.99413916173369
|
||||
-5.285711707241269,34.1685369626557
|
||||
-5.714255421479081,34.326879736594805
|
||||
-6.077686218342556,34.46827135542728
|
||||
|
||||
|
@ -3,7 +3,7 @@
|
||||
"scan_limit": 60,
|
||||
"max_range": 100,
|
||||
"geometry": "1200x1024+463+195",
|
||||
"last_selected_scenario": "scenario2",
|
||||
"last_selected_scenario": "scenario3",
|
||||
"connection": {
|
||||
"target": {
|
||||
"type": "tftp",
|
||||
@ -114,7 +114,7 @@
|
||||
"target_azimuth_deg": 45.0
|
||||
}
|
||||
],
|
||||
"use_spline": false
|
||||
"use_spline": true
|
||||
},
|
||||
{
|
||||
"target_id": 1,
|
||||
@ -150,6 +150,57 @@
|
||||
"target_azimuth_deg": -10.0
|
||||
}
|
||||
],
|
||||
"use_spline": true
|
||||
},
|
||||
{
|
||||
"target_id": 2,
|
||||
"active": true,
|
||||
"traceable": true,
|
||||
"trajectory": [
|
||||
{
|
||||
"maneuver_type": "Fly to Point",
|
||||
"duration_s": 1.0,
|
||||
"target_altitude_ft": 10000.0,
|
||||
"target_range_nm": 40.0,
|
||||
"target_azimuth_deg": 0.0
|
||||
},
|
||||
{
|
||||
"maneuver_type": "Fly for Duration",
|
||||
"target_velocity_fps": 506.343,
|
||||
"target_heading_deg": -180.0,
|
||||
"duration_s": 10.0,
|
||||
"target_altitude_ft": 10000.0
|
||||
},
|
||||
{
|
||||
"maneuver_type": "Dynamic Maneuver",
|
||||
"duration_s": 10.0,
|
||||
"target_altitude_ft": 10000.0,
|
||||
"dynamic_velocity_fps": 506.343,
|
||||
"lateral_g": 9.0,
|
||||
"longitudinal_g": 0.0,
|
||||
"vertical_g": 0.0
|
||||
}
|
||||
],
|
||||
"use_spline": false
|
||||
}
|
||||
]
|
||||
},
|
||||
"scenario3": {
|
||||
"name": "scenario3",
|
||||
"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
|
||||
}
|
||||
],
|
||||
"use_spline": false
|
||||
}
|
||||
]
|
||||
|
||||
@ -19,24 +19,27 @@ NM_TO_FT = 6076.12
|
||||
class ManeuverType(Enum):
|
||||
FLY_TO_POINT = "Fly to Point"
|
||||
FLY_FOR_DURATION = "Fly for Duration"
|
||||
DYNAMIC_MANEUVER = "Dynamic Maneuver"
|
||||
|
||||
@dataclass
|
||||
class Waypoint:
|
||||
"""Represents a segment of a target's trajectory, defining a maneuver."""
|
||||
maneuver_type: ManeuverType = ManeuverType.FLY_FOR_DURATION
|
||||
|
||||
# Parameters used by FLY_FOR_DURATION and as final state for FLY_TO_POINT
|
||||
# Parameters for FLY_FOR_DURATION and FLY_TO_POINT
|
||||
target_velocity_fps: Optional[float] = None
|
||||
target_heading_deg: Optional[float] = None
|
||||
|
||||
# Parameters used by both
|
||||
duration_s: Optional[float] = 10.0
|
||||
target_altitude_ft: Optional[float] = 10000.0
|
||||
|
||||
# Parameters used by FLY_TO_POINT
|
||||
target_range_nm: Optional[float] = None
|
||||
target_azimuth_deg: Optional[float] = None
|
||||
|
||||
# Parameters for DYNAMIC_MANEUVER (all obbligatori, velocity non opzionale)
|
||||
dynamic_velocity_fps: Optional[float] = None # velocità durante la manovra
|
||||
lateral_g: Optional[float] = None # G laterale (virata)
|
||||
longitudinal_g: Optional[float] = None # G longitudinale (accelerazione)
|
||||
vertical_g: Optional[float] = None # G verticale (salita/discesa)
|
||||
|
||||
# Internal state for interpolation, set when waypoint becomes active
|
||||
_start_velocity_fps: float = field(init=False, repr=False, default=0.0)
|
||||
_start_heading_deg: float = field(init=False, repr=False, default=0.0)
|
||||
@ -45,7 +48,6 @@ class Waypoint:
|
||||
|
||||
def to_dict(self) -> Dict:
|
||||
data = {"maneuver_type": self.maneuver_type.value}
|
||||
# Use dataclasses.fields to iterate reliably
|
||||
for f in fields(self):
|
||||
if not f.name.startswith('_') and f.name != "maneuver_type":
|
||||
val = getattr(self, f.name)
|
||||
@ -194,54 +196,9 @@ class Target:
|
||||
return
|
||||
|
||||
if self.use_spline and self._spline_path:
|
||||
self._sim_time_s += delta_time_s
|
||||
|
||||
total_duration = self._spline_point_times[-1]
|
||||
if total_duration <= 0 or self._sim_time_s >= total_duration:
|
||||
# Simulation finished, stay at the last point
|
||||
self._spline_index = len(self._spline_path) - 1
|
||||
self.active = False
|
||||
self.current_velocity_fps = 0.0
|
||||
else:
|
||||
# Find the current index in the spline path based on time
|
||||
progress = self._sim_time_s / total_duration
|
||||
self._spline_index = int(progress * (len(self._spline_path) - 1))
|
||||
|
||||
pt = self._spline_path[self._spline_index]
|
||||
|
||||
# Determine previous point for calculating heading/pitch
|
||||
prev_pt = self._spline_path[self._spline_index - 1] if self._spline_index > 0 else pt
|
||||
|
||||
# Update position (path is already in feet)
|
||||
self._pos_x_ft, self._pos_y_ft, self._pos_z_ft = pt[0], pt[1], pt[2]
|
||||
|
||||
# Update kinematics
|
||||
dx = self._pos_x_ft - prev_pt[0]
|
||||
dy = self._pos_y_ft - prev_pt[1]
|
||||
dz = self._pos_z_ft - prev_pt[2]
|
||||
|
||||
dist_moved_since_last_tick = math.sqrt(dx**2 + dy**2 + dz**2)
|
||||
dist_2d = math.sqrt(dx**2 + dy**2)
|
||||
|
||||
if dist_2d > 0.1: # Avoid division by zero or noise
|
||||
self.current_heading_deg = math.degrees(math.atan2(dx, dy)) % 360
|
||||
self.current_pitch_deg = math.degrees(math.atan2(dz, dist_2d))
|
||||
|
||||
if delta_time_s > 0:
|
||||
self.current_velocity_fps = dist_moved_since_last_tick / delta_time_s
|
||||
|
||||
self._update_current_polar_coords()
|
||||
|
||||
# --- DEBUG LOG ---
|
||||
#import logging
|
||||
#logging.getLogger("target_simulator.spline_debug").info(
|
||||
# f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}")
|
||||
# --- DEBUG LOG ---
|
||||
#import logging
|
||||
#logging.getLogger("target_simulator.spline_debug").info(
|
||||
# f"SplinePos: t={self._sim_time_s:.2f}s idx={self._spline_index} x={self._pos_x_ft:.2f} y={self._pos_y_ft:.2f} z={self._pos_z_ft:.2f}")
|
||||
# ...existing code...
|
||||
pass
|
||||
else:
|
||||
# Logica classica
|
||||
self._sim_time_s += delta_time_s
|
||||
self._time_in_waypoint_s += delta_time_s
|
||||
if self._current_waypoint_index == -1:
|
||||
@ -253,27 +210,56 @@ class Target:
|
||||
if wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
|
||||
self.current_velocity_fps = wp.target_velocity_fps or self.current_velocity_fps
|
||||
self.current_heading_deg = wp.target_heading_deg or self.current_heading_deg
|
||||
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
self.current_velocity_fps = wp.dynamic_velocity_fps or self.current_velocity_fps
|
||||
self.current_altitude_ft = wp.target_altitude_ft or self.current_altitude_ft
|
||||
self._activate_next_waypoint()
|
||||
if self._current_waypoint_index == -1:
|
||||
return
|
||||
wp = self.trajectory[self._current_waypoint_index] # Get new active waypoint
|
||||
wp = self.trajectory[self._current_waypoint_index]
|
||||
duration = wp._calculated_duration_s if wp._calculated_duration_s is not None else wp.duration_s or 0.0
|
||||
progress = self._time_in_waypoint_s / duration if duration > 0 else 1.0
|
||||
if wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
|
||||
# Interpolate kinematics during the maneuver
|
||||
# ...existing code...
|
||||
self.current_velocity_fps = wp._start_velocity_fps + ((wp.target_velocity_fps or wp._start_velocity_fps) - wp._start_velocity_fps) * progress
|
||||
self.current_altitude_ft = wp._start_altitude_ft + ((wp.target_altitude_ft or wp._start_altitude_ft) - wp._start_altitude_ft) * progress
|
||||
delta_heading = ((wp.target_heading_deg or wp._start_heading_deg) - wp._start_heading_deg + 180) % 360 - 180
|
||||
self.current_heading_deg = (wp._start_heading_deg + delta_heading * progress) % 360
|
||||
# Update position using current 3D vector
|
||||
heading_rad = math.radians(self.current_heading_deg)
|
||||
pitch_rad = math.radians(self.current_pitch_deg)
|
||||
dist_moved = self.current_velocity_fps * delta_time_s
|
||||
dist_2d = dist_moved * math.cos(pitch_rad)
|
||||
self._pos_x_ft += dist_2d * math.sin(heading_rad)
|
||||
self._pos_y_ft += dist_2d * math.cos(heading_rad)
|
||||
self._pos_z_ft += dist_moved * math.sin(pitch_rad)
|
||||
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
# Logica dinamica: integra accelerazioni e heading
|
||||
# Parametri obbligatori: dynamic_velocity_fps, lateral_g, duration_s
|
||||
velocity = wp.dynamic_velocity_fps
|
||||
lateral_g = wp.lateral_g or 0.0
|
||||
longitudinal_g = wp.longitudinal_g or 0.0
|
||||
vertical_g = wp.vertical_g or 0.0
|
||||
# Conversione G in ft/s^2
|
||||
G_FT_S2 = 32.174
|
||||
lateral_acc = lateral_g * G_FT_S2
|
||||
long_acc = longitudinal_g * G_FT_S2
|
||||
vert_acc = vertical_g * G_FT_S2
|
||||
# Aggiorna heading (virata): rateo di virata = a_laterale / velocità
|
||||
# LV turn_rate_rad_s = lateral_acc / max(velocity, 1e-3)
|
||||
|
||||
R = (velocity* velocity)/lateral_acc
|
||||
omega = velocity/R if R!=0 else 0
|
||||
turn_rate_rad_s = omega
|
||||
|
||||
|
||||
self.current_heading_deg = (self.current_heading_deg + math.degrees(turn_rate_rad_s * delta_time_s)) % 360
|
||||
|
||||
|
||||
|
||||
# Aggiorna velocità (accelerazione longitudinale)
|
||||
self.current_velocity_fps = velocity + long_acc * delta_time_s
|
||||
# Aggiorna posizione
|
||||
heading_rad = math.radians(self.current_heading_deg)
|
||||
pitch_rad = math.radians(self.current_pitch_deg)
|
||||
dist_moved = self.current_velocity_fps * delta_time_s
|
||||
dist_2d = dist_moved * math.cos(pitch_rad)
|
||||
self._pos_x_ft += dist_2d * math.sin(heading_rad)
|
||||
self._pos_y_ft += dist_2d * math.cos(heading_rad)
|
||||
self._pos_z_ft += dist_moved * math.sin(pitch_rad) + vert_acc * delta_time_s
|
||||
# ...existing code...
|
||||
self._update_current_polar_coords()
|
||||
|
||||
def _update_current_polar_coords(self):
|
||||
|
||||
@ -160,15 +160,122 @@ class PPIDisplay(ttk.Frame):
|
||||
Simulates and draws a trajectory preview.
|
||||
Accepts either a list of Waypoint or a Target object.
|
||||
"""
|
||||
# Pulisci tutto prima di ridisegnare
|
||||
self.clear_previews()
|
||||
self._waypoints_plot.set_data([], [])
|
||||
self._start_plot.set_data([], [])
|
||||
if hasattr(self, '_preview_artist'):
|
||||
self._preview_artist.set_data([], [])
|
||||
# Forza la pulizia della canvas
|
||||
self.canvas.draw()
|
||||
self._on_range_selected()
|
||||
|
||||
# --- Simula la traiettoria anche per manovre dinamiche ---
|
||||
waypoints = trajectory if isinstance(trajectory, list) else getattr(trajectory, 'trajectory', [])
|
||||
preview_points = []
|
||||
# Stato iniziale
|
||||
pos_x, pos_y, pos_z = 0.0, 0.0, 0.0
|
||||
heading_deg = 0.0
|
||||
velocity_fps = 0.0
|
||||
altitude_ft = 0.0
|
||||
# Usa il primo waypoint come stato iniziale se disponibile
|
||||
if waypoints:
|
||||
wp0 = waypoints[0]
|
||||
if getattr(wp0, 'target_range_nm', None) is not None and getattr(wp0, 'target_azimuth_deg', None) is not None:
|
||||
r0 = wp0.target_range_nm
|
||||
th0 = math.radians(wp0.target_azimuth_deg)
|
||||
pos_x = r0 * 6076.12 * math.sin(th0)
|
||||
pos_y = r0 * 6076.12 * math.cos(th0)
|
||||
pos_z = wp0.target_altitude_ft or 0.0
|
||||
heading_deg = wp0.target_heading_deg or 0.0
|
||||
velocity_fps = wp0.target_velocity_fps or 0.0
|
||||
altitude_ft = pos_z
|
||||
preview_points.append((pos_x, pos_y))
|
||||
# Simula ogni waypoint
|
||||
for wp in waypoints:
|
||||
if wp.maneuver_type == ManeuverType.FLY_TO_POINT:
|
||||
if wp.target_range_nm is not None and wp.target_azimuth_deg is not None:
|
||||
r = wp.target_range_nm
|
||||
th = math.radians(wp.target_azimuth_deg)
|
||||
pos_x = r * 6076.12 * math.sin(th)
|
||||
pos_y = r * 6076.12 * math.cos(th)
|
||||
pos_z = wp.target_altitude_ft or pos_z
|
||||
heading_deg = wp.target_heading_deg or heading_deg
|
||||
velocity_fps = wp.target_velocity_fps or velocity_fps
|
||||
preview_points.append((pos_x, pos_y))
|
||||
elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
|
||||
# Interpolazione lineare
|
||||
dt = wp.duration_s or 1.0
|
||||
steps = max(10, int(dt))
|
||||
for i in range(steps):
|
||||
frac = (i + 1) / steps
|
||||
heading_rad = math.radians(heading_deg)
|
||||
dist = (wp.target_velocity_fps or velocity_fps) * (dt / steps)
|
||||
pos_x += dist * math.sin(heading_rad)
|
||||
pos_y += dist * math.cos(heading_rad)
|
||||
preview_points.append((pos_x, pos_y))
|
||||
heading_deg = wp.target_heading_deg or heading_deg
|
||||
velocity_fps = wp.target_velocity_fps or velocity_fps
|
||||
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
# Simula la curva: integra accelerazione laterale
|
||||
dt = wp.duration_s or 1.0
|
||||
steps = max(20, int(dt * 2))
|
||||
velocity = wp.dynamic_velocity_fps or velocity_fps
|
||||
heading = heading_deg
|
||||
lateral_g = wp.lateral_g or 0.0
|
||||
long_g = wp.longitudinal_g or 0.0
|
||||
vert_g = wp.vertical_g or 0.0
|
||||
G_FT_S2 = 32.174
|
||||
lateral_acc = lateral_g * G_FT_S2
|
||||
long_acc = long_g * G_FT_S2
|
||||
vert_acc = vert_g * G_FT_S2
|
||||
for i in range(steps):
|
||||
dts = dt / steps
|
||||
# Aggiorna heading (virata)
|
||||
turn_rate_rad_s = lateral_acc / max(velocity, 1e-3)
|
||||
heading += math.degrees(turn_rate_rad_s * dts)
|
||||
heading_rad = math.radians(heading)
|
||||
# Aggiorna velocità
|
||||
velocity += long_acc * dts
|
||||
# Aggiorna posizione
|
||||
pos_x += velocity * math.sin(heading_rad) * dts
|
||||
pos_y += velocity * math.cos(heading_rad) * dts
|
||||
pos_z += vert_acc * dts
|
||||
preview_points.append((pos_x, pos_y))
|
||||
heading_deg = heading
|
||||
velocity_fps = velocity
|
||||
# Converti i punti in polari per la PPI
|
||||
thetas = []
|
||||
rs = []
|
||||
for x, y in preview_points:
|
||||
r = math.sqrt(x**2 + y**2) / 6076.12
|
||||
th = math.atan2(x, y)
|
||||
rs.append(r)
|
||||
thetas.append(th)
|
||||
|
||||
# Mostra il punto iniziale anche se c'è solo un waypoint
|
||||
if len(thetas) == 1:
|
||||
self._preview_artist.set_data([], [])
|
||||
self._waypoints_plot.set_data(thetas, rs)
|
||||
self._start_plot.set_data([thetas[0]], [rs[0]])
|
||||
heading_len = 3
|
||||
heading_theta = thetas[0]
|
||||
heading_r2 = rs[0] + heading_len
|
||||
if hasattr(self, '_heading_artist'):
|
||||
self._heading_artist.remove()
|
||||
self._heading_artist, = self.ax.plot([heading_theta, heading_theta], [rs[0], heading_r2], color='red', linewidth=1, alpha=0.8)
|
||||
self.canvas.draw()
|
||||
self._on_range_selected()
|
||||
return
|
||||
elif len(thetas) >= 2:
|
||||
self._preview_artist.set_data(thetas, rs)
|
||||
self._waypoints_plot.set_data(thetas, rs)
|
||||
self._start_plot.set_data([thetas[0]], [rs[0]])
|
||||
heading_len = 3
|
||||
heading_theta = thetas[0]
|
||||
heading_r2 = rs[0] + heading_len
|
||||
if hasattr(self, '_heading_artist'):
|
||||
self._heading_artist.remove()
|
||||
self._heading_artist, = self.ax.plot([heading_theta, heading_theta], [rs[0], heading_r2], color='red', linewidth=1, alpha=0.8)
|
||||
self.canvas.draw()
|
||||
self._on_range_selected()
|
||||
return
|
||||
|
||||
# Determine input type
|
||||
if isinstance(trajectory, list):
|
||||
@ -182,8 +289,15 @@ class PPIDisplay(ttk.Frame):
|
||||
return
|
||||
# Classic preview (polyline) SOLO se spline non attiva
|
||||
# (la preview spline cancella la classica)
|
||||
thetas = [math.radians(getattr(wp, 'target_azimuth_deg', 0)) for wp in waypoints]
|
||||
rs = [getattr(wp, 'target_range_nm', 0) for wp in waypoints]
|
||||
# Solo waypoint con azimuth e range (escludi manovre dinamiche)
|
||||
thetas = []
|
||||
rs = []
|
||||
for wp in waypoints:
|
||||
az = getattr(wp, 'target_azimuth_deg', None)
|
||||
rng = getattr(wp, 'target_range_nm', None)
|
||||
if az is not None and rng is not None:
|
||||
thetas.append(math.radians(az))
|
||||
rs.append(rng)
|
||||
if len(thetas) < 2:
|
||||
self.canvas.draw()
|
||||
return
|
||||
|
||||
@ -2,6 +2,7 @@ import tkinter as tk
|
||||
from tkinter import ttk, messagebox
|
||||
from typing import List, Optional
|
||||
import copy
|
||||
import math
|
||||
from queue import Queue, Empty
|
||||
|
||||
# Use absolute imports
|
||||
@ -40,7 +41,7 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
scenario_name = f"Trajectory_{self.target_id}"
|
||||
self.title(f"Trajectory Editor - {scenario_name} - Target {self.target_id}")
|
||||
|
||||
self.geometry("1100x700")
|
||||
self.geometry("1100x800")
|
||||
|
||||
self.gui_update_queue = Queue()
|
||||
self.preview_engine: Optional[SimulationEngine] = None
|
||||
@ -83,6 +84,11 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
self.ppi_preview = PPIDisplay(preview_frame, max_range_nm=self.initial_max_range)
|
||||
self.ppi_preview.pack(fill=tk.BOTH, expand=True)
|
||||
|
||||
# --- Parametri fisici manovra dinamica ---
|
||||
self.phys_params_label = ttk.Label(preview_frame, text="", font=("Consolas", 10), foreground="#00FF99")
|
||||
self.phys_params_label.pack(fill=tk.X, padx=5, pady=(0, 5))
|
||||
|
||||
# --- Controlli di simulazione ---
|
||||
preview_controls = ttk.Frame(preview_frame)
|
||||
preview_controls.pack(fill=tk.X, pady=5)
|
||||
self.play_button = ttk.Button(preview_controls, text="▶ Play", command=self._on_preview_play)
|
||||
@ -93,7 +99,6 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
self.stop_button.pack(side=tk.LEFT, padx=5)
|
||||
self.reset_button = ttk.Button(preview_controls, text="⟲ Reset", command=self._on_preview_reset)
|
||||
self.reset_button.pack(side=tk.LEFT, padx=5)
|
||||
# ...existing code...
|
||||
ttk.Label(preview_controls, text="Speed:").pack(side=tk.LEFT, padx=(10, 2), pady=5)
|
||||
self.time_multiplier_var = tk.StringVar(value="1x")
|
||||
self.multiplier_combo = ttk.Combobox(preview_controls, textvariable=self.time_multiplier_var, values=["1x", "2x", "4x", "10x"], state="readonly", width=4)
|
||||
@ -109,10 +114,65 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
self.sim_time_label = ttk.Label(progress_frame, text="0.0s / 0.0s")
|
||||
self.sim_time_label.pack(side=tk.RIGHT, padx=5)
|
||||
|
||||
# --- Pulsanti OK e Close ---
|
||||
button_frame = ttk.Frame(self)
|
||||
button_frame.pack(fill=tk.X, padx=10, pady=(0, 10), side=tk.BOTTOM)
|
||||
ttk.Button(button_frame, text="Cancel", command=self._on_cancel).pack(side=tk.RIGHT)
|
||||
ttk.Button(button_frame, text="OK", command=self._on_ok).pack(side=tk.RIGHT, padx=5)
|
||||
button_frame.pack(side=tk.BOTTOM, fill=tk.X, padx=10, pady=10)
|
||||
self.ok_button = ttk.Button(button_frame, text="OK", command=self._on_ok)
|
||||
self.ok_button.pack(side=tk.RIGHT, padx=5)
|
||||
self.close_button = ttk.Button(button_frame, text="Close", command=self._on_cancel)
|
||||
self.close_button.pack(side=tk.RIGHT, padx=5)
|
||||
def _show_physical_params(self):
|
||||
selected = self.wp_tree.focus()
|
||||
if not selected:
|
||||
self.phys_params_label.config(text="")
|
||||
return
|
||||
try:
|
||||
idx = int(selected)
|
||||
wp = self.waypoints[idx]
|
||||
if wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
vel_kn = (wp.dynamic_velocity_fps or 0.0) * FPS_TO_KNOTS
|
||||
vel_mps = vel_kn * 0.514444
|
||||
g = wp.lateral_g or 0.0
|
||||
g_acc = 9.81
|
||||
if g != 0 and vel_mps > 0:
|
||||
radius_m = vel_mps ** 2 / (g_acc * abs(g))
|
||||
turn_rate_deg_s = (g_acc * abs(g)) / vel_mps * (180 / math.pi)
|
||||
self.phys_params_label.config(
|
||||
text=f"Raggio curvatura: {radius_m:,.0f} m | Rateo virata: {turn_rate_deg_s:.1f}°/s | Velocità: {vel_kn:.1f} kn | G lat: {g:.2f}"
|
||||
)
|
||||
else:
|
||||
self.phys_params_label.config(text="(Parametri insufficienti per calcolo fisico)")
|
||||
else:
|
||||
self.phys_params_label.config(text="")
|
||||
except Exception:
|
||||
self.phys_params_label.config(text="")
|
||||
def _show_physical_params(self):
|
||||
selected = self.wp_tree.focus()
|
||||
if not selected:
|
||||
self.phys_params_label.config(text="")
|
||||
return
|
||||
try:
|
||||
idx = int(selected)
|
||||
wp = self.waypoints[idx]
|
||||
if wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
vel_kn = (wp.dynamic_velocity_fps or 0.0) * FPS_TO_KNOTS
|
||||
vel_mps = vel_kn * 0.514444
|
||||
g = wp.lateral_g or 0.0
|
||||
g_acc = 9.81
|
||||
if g != 0 and vel_mps > 0:
|
||||
radius_m = vel_mps ** 2 / (g_acc * abs(g))
|
||||
turn_rate_deg_s = (g_acc * abs(g)) / vel_mps * (180 / math.pi)
|
||||
self.phys_params_label.config(
|
||||
text=f"Raggio curvatura: {radius_m:,.0f} m | Rateo virata: {turn_rate_deg_s:.1f}°/s | Velocità: {vel_kn:.1f} kn | G lat: {g:.2f}"
|
||||
)
|
||||
else:
|
||||
self.phys_params_label.config(text="(Parametri insufficienti per calcolo fisico)")
|
||||
else:
|
||||
self.phys_params_label.config(text="")
|
||||
except Exception:
|
||||
self.phys_params_label.config(text="")
|
||||
|
||||
# Questa funzione deve solo aggiornare la label dei parametri fisici
|
||||
def _on_spline_toggle(self):
|
||||
self._update_static_preview()
|
||||
|
||||
@ -134,6 +194,7 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
self.wp_tree.column("details", width=250)
|
||||
self.wp_tree.pack(side=tk.TOP, fill=tk.BOTH, expand=True)
|
||||
self.wp_tree.bind('<Double-1>', self._on_edit_waypoint)
|
||||
self.wp_tree.bind('<<TreeviewSelect>>', lambda e: self._show_physical_params())
|
||||
|
||||
btn_frame = ttk.Frame(parent)
|
||||
btn_frame.pack(fill=tk.X, pady=5)
|
||||
@ -146,6 +207,8 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
editor = WaypointEditorWindow(self, is_first_waypoint=True)
|
||||
if editor.result_waypoint:
|
||||
self.waypoints.append(editor.result_waypoint)
|
||||
self._populate_waypoint_list()
|
||||
self._update_static_preview()
|
||||
else:
|
||||
# If user cancels creation of the initial point, close the whole editor
|
||||
self.after(10, self._on_cancel)
|
||||
@ -159,8 +222,13 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
elif wp.maneuver_type == ManeuverType.FLY_FOR_DURATION:
|
||||
vel_kn = (wp.target_velocity_fps or 0.0) * FPS_TO_KNOTS
|
||||
details = f"Vel:{vel_kn:.1f}kn, Hdg:{wp.target_heading_deg:.1f}°, T:{wp.duration_s:.1f}s"
|
||||
|
||||
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
vel_kn = (wp.dynamic_velocity_fps or 0.0) * FPS_TO_KNOTS
|
||||
details = (
|
||||
f"DynVel:{vel_kn:.1f}kn, LatG:{wp.lateral_g:.2f}, LongG:{wp.longitudinal_g:.2f}, VertG:{wp.vertical_g:.2f}, T:{wp.duration_s:.1f}s"
|
||||
)
|
||||
self.wp_tree.insert("", tk.END, iid=str(i), values=(i, wp.maneuver_type.value, details))
|
||||
self._show_physical_params()
|
||||
|
||||
def _on_add_waypoint(self):
|
||||
editor = WaypointEditorWindow(self, is_first_waypoint=False)
|
||||
@ -171,6 +239,7 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
# Select the newly added item
|
||||
new_item_id = str(len(self.waypoints) - 1)
|
||||
self.wp_tree.focus(new_item_id); self.wp_tree.selection_set(new_item_id)
|
||||
self._show_physical_params()
|
||||
|
||||
def _on_edit_waypoint(self, event=None):
|
||||
selected_item = self.wp_tree.focus()
|
||||
@ -190,6 +259,7 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
self._populate_waypoint_list()
|
||||
self._update_static_preview()
|
||||
self.wp_tree.focus(selected_item); self.wp_tree.selection_set(selected_item)
|
||||
self._show_physical_params()
|
||||
|
||||
def _on_remove_waypoint(self):
|
||||
selected_item = self.wp_tree.focus()
|
||||
@ -206,6 +276,7 @@ class TrajectoryEditorWindow(tk.Toplevel):
|
||||
del self.waypoints[wp_index]
|
||||
self._populate_waypoint_list()
|
||||
self._update_static_preview()
|
||||
self._show_physical_params()
|
||||
|
||||
def _update_static_preview(self):
|
||||
"""Draws the static trajectory path on the PPI."""
|
||||
|
||||
@ -45,6 +45,11 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
self.t_alt_var = tk.DoubleVar(value=10000.0)
|
||||
self.t_vel_var = tk.DoubleVar(value=300.0)
|
||||
self.t_hdg_var = tk.DoubleVar(value=90.0)
|
||||
# Dynamic maneuver variables
|
||||
self.dyn_vel_var = tk.DoubleVar(value=300.0)
|
||||
self.lateral_g_var = tk.DoubleVar(value=0.0)
|
||||
self.longitudinal_g_var = tk.DoubleVar(value=0.0)
|
||||
self.vertical_g_var = tk.DoubleVar(value=0.0)
|
||||
|
||||
def _load_waypoint_data(self, wp: Waypoint):
|
||||
"""Populates the fields with data from an existing waypoint."""
|
||||
@ -55,6 +60,11 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
self.t_alt_var.set(wp.target_altitude_ft or 10000.0)
|
||||
self.t_vel_var.set((wp.target_velocity_fps or 0.0) * FPS_TO_KNOTS)
|
||||
self.t_hdg_var.set(wp.target_heading_deg or 0.0)
|
||||
# Carica i parametri dinamici se presenti
|
||||
self.dyn_vel_var.set((getattr(wp, 'dynamic_velocity_fps', 0.0) or 0.0) * FPS_TO_KNOTS)
|
||||
self.lateral_g_var.set(getattr(wp, 'lateral_g', 0.0) or 0.0)
|
||||
self.longitudinal_g_var.set(getattr(wp, 'longitudinal_g', 0.0) or 0.0)
|
||||
self.vertical_g_var.set(getattr(wp, 'vertical_g', 0.0) or 0.0)
|
||||
|
||||
def _center_window(self):
|
||||
self.update_idletasks()
|
||||
@ -85,6 +95,7 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
# --- Dynamic Frames ---
|
||||
self.fly_to_point_frame = self._create_fly_to_point_frame(main_frame)
|
||||
self.fly_for_duration_frame = self._create_fly_for_duration_frame(main_frame)
|
||||
self.dynamic_maneuver_frame = self._create_dynamic_maneuver_frame(main_frame)
|
||||
|
||||
# --- Buttons ---
|
||||
button_frame = ttk.Frame(main_frame)
|
||||
@ -100,6 +111,20 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
self.maneuver_type_var.set(ManeuverType.FLY_FOR_DURATION.value)
|
||||
|
||||
self._on_maneuver_type_change()
|
||||
def _create_dynamic_maneuver_frame(self, parent) -> ttk.Frame:
|
||||
frame = ttk.Frame(parent, padding=10)
|
||||
frame.columnconfigure(1, weight=1)
|
||||
ttk.Label(frame, text="Duration (s):").grid(row=0, column=0, sticky=tk.W, pady=2)
|
||||
ttk.Spinbox(frame, from_=0.1, to=3600, textvariable=self.duration_var, width=10).grid(row=0, column=1, sticky=tk.EW, pady=2)
|
||||
ttk.Label(frame, text="Velocity (kn):").grid(row=1, column=0, sticky=tk.W, pady=2)
|
||||
ttk.Spinbox(frame, from_=0, to=2000, textvariable=self.dyn_vel_var, width=10).grid(row=1, column=1, sticky=tk.EW, pady=2)
|
||||
ttk.Label(frame, text="Lateral G:").grid(row=2, column=0, sticky=tk.W, pady=2)
|
||||
ttk.Spinbox(frame, from_=-12, to=12, textvariable=self.lateral_g_var, width=10, increment=0.1).grid(row=2, column=1, sticky=tk.EW, pady=2)
|
||||
ttk.Label(frame, text="Longitudinal G:").grid(row=3, column=0, sticky=tk.W, pady=2)
|
||||
ttk.Spinbox(frame, from_=-5, to=5, textvariable=self.longitudinal_g_var, width=10, increment=0.1).grid(row=3, column=1, sticky=tk.EW, pady=2)
|
||||
ttk.Label(frame, text="Vertical G:").grid(row=4, column=0, sticky=tk.W, pady=2)
|
||||
ttk.Spinbox(frame, from_=-5, to=5, textvariable=self.vertical_g_var, width=10, increment=0.1).grid(row=4, column=1, sticky=tk.EW, pady=2)
|
||||
return frame
|
||||
|
||||
def _create_fly_to_point_frame(self, parent) -> ttk.Frame:
|
||||
frame = ttk.Frame(parent, padding=10)
|
||||
@ -131,18 +156,20 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
|
||||
def _on_maneuver_type_change(self, event=None):
|
||||
m_type = ManeuverType(self.maneuver_type_var.get())
|
||||
self.fly_to_point_frame.pack_forget()
|
||||
self.fly_for_duration_frame.pack_forget()
|
||||
self.dynamic_maneuver_frame.pack_forget()
|
||||
if m_type == ManeuverType.FLY_TO_POINT:
|
||||
self.fly_for_duration_frame.pack_forget()
|
||||
self.fly_to_point_frame.pack(fill=tk.BOTH, expand=True)
|
||||
else: # Fly for duration
|
||||
self.fly_to_point_frame.pack_forget()
|
||||
elif m_type == ManeuverType.FLY_FOR_DURATION:
|
||||
self.fly_for_duration_frame.pack(fill=tk.BOTH, expand=True)
|
||||
elif m_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
self.dynamic_maneuver_frame.pack(fill=tk.BOTH, expand=True)
|
||||
|
||||
def _on_ok(self):
|
||||
try:
|
||||
m_type = ManeuverType(self.maneuver_type_var.get())
|
||||
wp = Waypoint(maneuver_type=m_type)
|
||||
|
||||
if m_type == ManeuverType.FLY_TO_POINT:
|
||||
wp.target_range_nm = self.t_range_var.get()
|
||||
wp.target_azimuth_deg = self.t_az_var.get()
|
||||
@ -152,10 +179,14 @@ class WaypointEditorWindow(tk.Toplevel):
|
||||
wp.duration_s = self.duration_var.get()
|
||||
wp.target_velocity_fps = self.t_vel_var.get() * KNOTS_TO_FPS
|
||||
wp.target_heading_deg = self.t_hdg_var.get()
|
||||
# Also save altitude if it's the first waypoint
|
||||
if self.is_first_waypoint:
|
||||
wp.target_altitude_ft = self.t_alt_var.get()
|
||||
|
||||
elif m_type == ManeuverType.DYNAMIC_MANEUVER:
|
||||
wp.duration_s = self.duration_var.get()
|
||||
wp.dynamic_velocity_fps = self.dyn_vel_var.get() * KNOTS_TO_FPS
|
||||
wp.lateral_g = self.lateral_g_var.get()
|
||||
wp.longitudinal_g = self.longitudinal_g_var.get()
|
||||
wp.vertical_g = self.vertical_g_var.get()
|
||||
self.result_waypoint = wp
|
||||
self.destroy()
|
||||
except (ValueError, tk.TclError) as e:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user