235 lines
11 KiB
Python
235 lines
11 KiB
Python
from ..lib1553 import messages as msgs
|
|
from ..lib1553.constants import (
|
|
TargetHistory, RadarMode, AltitudeBlock,
|
|
RWSSubmode, GMSubmode, RangeScale, BarScan, AzimuthScan,
|
|
StandbyStatus, DesignationControl
|
|
)
|
|
import logging
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
class RadarController:
|
|
"""
|
|
High-level controller for the Radar state.
|
|
|
|
FUTURE EXTENSION POINTS:
|
|
- add validate_command(msg, field, value) for ICD range checking
|
|
- add queue_command(msg) for rate-limited transmission
|
|
- add apply_config(profile) to replace hardcoded defaults
|
|
- add state change callbacks for GUI synchronization
|
|
Holds the instances of 1553 messages (A-Transmit / B-Receive).
|
|
Acts as the interface for the GUI or Scripts to modify radar behavior.
|
|
"""
|
|
def __init__(self):
|
|
# --- TX Messages (BC -> RT) ---
|
|
# These are the commands we send to the Radar
|
|
self.msg_a1 = msgs.MsgA1() # Settings
|
|
self.msg_a2 = msgs.MsgA2() # Commands
|
|
self.msg_a3 = msgs.MsgA3() # Graphics
|
|
self.msg_a4 = msgs.MsgA4() # Nav Data
|
|
self.msg_a5 = msgs.MsgA5() # INU Data
|
|
self.msg_a7 = msgs.MsgA7() # Data Link 1
|
|
self.msg_a8 = msgs.MsgA8() # Data Link 2
|
|
|
|
# --- RX Messages (RT -> BC) ---
|
|
# These are placeholders/requests we send to get data back
|
|
self.msg_b1 = msgs.MsgB1() # TWS 1-2
|
|
self.msg_b2 = msgs.MsgB2() # TWS 3-5
|
|
self.msg_b3 = msgs.MsgB3() # TWS 6-8
|
|
self.msg_b4 = msgs.MsgB4() # SPT
|
|
self.msg_b5 = msgs.MsgB5() # Tracked Target
|
|
self.msg_b6 = msgs.MsgB6() # Settings Tell-back
|
|
self.msg_b7 = msgs.MsgB7() # Status Tell-back
|
|
self.msg_b8 = msgs.MsgB8() # BIT Report
|
|
|
|
# NOTE: Defaults are NOT applied automatically
|
|
# Call apply_defaults() explicitly when needed (e.g., when RUN button is pressed)
|
|
|
|
def apply_defaults(self):
|
|
"""
|
|
Sets the default values for the radar commands.
|
|
Based on legacy initialization logic from working test_1553.py.
|
|
|
|
NOTE: This sets STANDBY=ON initially for safe startup.
|
|
Call apply_startup_sequence() to properly initialize the radar.
|
|
"""
|
|
logger.info("Applying default radar configuration...")
|
|
|
|
# --- A1: Settings ---
|
|
# Legacy: settings.set_history_level(TARGET_HISTORY_LEVEL_04)
|
|
self.msg_a1.target_history = TargetHistory.LEVEL_4
|
|
|
|
# Legacy: rdr_symbology_intensity = 127
|
|
self.msg_a1.symbol_intensity = 127
|
|
|
|
# Additional safe defaults for A1
|
|
self.msg_a1.altitude_block = AltitudeBlock.NORMAL
|
|
self.msg_a1.beacon_delay = 0.0
|
|
|
|
# Frequency agility and other A1 fields initialized to 0 by default (safe)
|
|
|
|
# --- A2: Operation Command ---
|
|
# Legacy: set_master_mode(RWS)
|
|
self.msg_a2.master_mode = RadarMode.RWS
|
|
|
|
# CRITICAL: Start with STANDBY=ON for safe initialization (C++ legacy behavior)
|
|
# This will be disabled in Phase 2 of startup sequence
|
|
self.msg_a2.standby_cmd = StandbyStatus.ON
|
|
|
|
# Legacy: set_gm_submode(0) -> RBM
|
|
self.msg_a2.gm_submode = GMSubmode.RBM
|
|
|
|
# Ensure other modes are in a known state
|
|
self.msg_a2.rws_submode = RWSSubmode.NAM
|
|
|
|
# Set reasonable scan parameters for RWS mode
|
|
self.msg_a2.range_scale = RangeScale.NM_20 # 20 NM default
|
|
self.msg_a2.bar_scan = BarScan.BAR_1 # 1 bar
|
|
self.msg_a2.azimuth_scan = AzimuthScan.DEG_60 # 60 deg scan
|
|
|
|
# Designation control = NOT_VALID (no designation active)
|
|
self.msg_a2.designation_ctrl = DesignationControl.NOT_VALID
|
|
|
|
# CRITICAL: Set SILENCE=1 for safe initialization (C++ legacy: dSilence.setFromUser(1))
|
|
# Prevents RF radiation during startup
|
|
self.msg_a2.silence_reserved = 1
|
|
|
|
# --- A3: Graphic ---
|
|
# Legacy: just instantiated (all zeros)
|
|
# A3 controls symbology display. 0 usually means nothing displayed initially.
|
|
# Can be configured via GUI later if needed.
|
|
|
|
# --- A4: Nav Data ---
|
|
# Legacy: validity_and_slew.raw = 0
|
|
# CRITICAL: validity bits = 0 means data is VALID (inverse logic!)
|
|
# Set all validity flags to 0 (data valid)
|
|
self.msg_a4.nav_data_invalid = 0
|
|
self.msg_a4.attitude_data_invalid = 0
|
|
self.msg_a4.baro_inertial_alt_invalid = 0
|
|
self.msg_a4.corr_baro_alt_invalid = 0
|
|
self.msg_a4.radalt_invalid = 0
|
|
self.msg_a4.spoi_alt_invalid = 0
|
|
self.msg_a4.spoi_pos_invalid = 0
|
|
self.msg_a4.tas_invalid = 0
|
|
self.msg_a4.cas_invalid = 0
|
|
self.msg_a4.ppos_invalid = 0
|
|
self.msg_a4.cursor_rates_invalid = 0
|
|
|
|
# Provide some dummy nav data (radar needs valid nav context)
|
|
self.msg_a4.baro_inertial_alt = 10000.0 # 10,000 ft
|
|
self.msg_a4.tas = 300.0 # 300 knots
|
|
self.msg_a4.cas = 250 # 250 knots
|
|
self.msg_a4.true_heading = 0.0 # North
|
|
|
|
# --- A5: INU Data ---
|
|
# Legacy: timetag.raw = 0
|
|
self.msg_a5.time_tag = 0.0
|
|
|
|
# Note: velocity_x/y/z are read-only properties (32-bit fields)
|
|
# They're calculated from underlying words, initialized to 0 by default
|
|
# To set them, we'd need to write directly to _data[2-7]
|
|
# For now, leave as 0 (default initialization is fine)
|
|
|
|
# Angular rates (zero = stable platform) - these ARE settable
|
|
self.msg_a5.pitch_rate = 0.0
|
|
self.msg_a5.roll_rate = 0.0
|
|
self.msg_a5.yaw_rate = 0.0
|
|
|
|
logger.info("Radar defaults applied (PHASE 1 - Protection Mode):")
|
|
logger.info(f" → Master Mode: {self.msg_a2.master_mode.name}")
|
|
logger.info(f" → Standby: {self.msg_a2.standby_cmd.name} (ON for safe startup)")
|
|
logger.info(f" → Silence: {self.msg_a2.silence_reserved} (ON for RF protection)")
|
|
logger.info(f" → Range Scale: {self.msg_a2.range_scale.name}")
|
|
logger.info(f" → Nav Data Valid: YES (all validity flags = 0)")
|
|
logger.info(f" → Altitude: {self.msg_a4.baro_inertial_alt} ft")
|
|
logger.info(f" → TAS: {self.msg_a4.tas} kts")
|
|
logger.info("")
|
|
logger.info("⚠️ Use apply_startup_sequence() to complete radar initialization")
|
|
|
|
# Log critical A2 command values for debugging
|
|
logger.debug("=== A2 Command Details ===")
|
|
logger.debug(f" master_mode = {self.msg_a2.master_mode.name} ({self.msg_a2.master_mode.value})")
|
|
logger.debug(f" standby_cmd = {self.msg_a2.standby_cmd.name} ({self.msg_a2.standby_cmd.value})")
|
|
logger.debug(f" rws_submode = {self.msg_a2.rws_submode.name} ({self.msg_a2.rws_submode.value})")
|
|
logger.debug(f" gm_submode = {self.msg_a2.gm_submode.name} ({self.msg_a2.gm_submode.value})")
|
|
logger.debug(f" range_scale = {self.msg_a2.range_scale.name} ({self.msg_a2.range_scale.value})")
|
|
logger.debug(f" bar_scan = {self.msg_a2.bar_scan.name} ({self.msg_a2.bar_scan.value})")
|
|
logger.debug(f" azimuth_scan = {self.msg_a2.azimuth_scan.name} ({self.msg_a2.azimuth_scan.value})")
|
|
logger.debug(f" designation_ctrl = {self.msg_a2.designation_ctrl.name} ({self.msg_a2.designation_ctrl.value})")
|
|
|
|
# Log raw data word 0 (critical - contains master_mode and standby)
|
|
word0 = self.msg_a2._data[0]
|
|
logger.debug(f" A2 Word[0] RAW = 0x{word0:04X} (binary: {word0:016b})")
|
|
# Extract using ICD bit numbering (MSB=0): shift = 16 - (start_bit + width)
|
|
logger.debug(f" Bits 0-3 (master_mode) = {(word0 >> 12) & 0xF:04b} = {(word0 >> 12) & 0xF}")
|
|
logger.debug(f" Bit 9 (standby_cmd) = {(word0 >> 6) & 1}")
|
|
|
|
# Log A4 nav data
|
|
logger.debug("=== A4 Nav Data ===")
|
|
logger.debug(f" altitude = {self.msg_a4.baro_inertial_alt} ft")
|
|
logger.debug(f" tas = {self.msg_a4.tas} kts")
|
|
logger.debug(f" cas = {self.msg_a4.cas} kts")
|
|
logger.debug(f" heading = {self.msg_a4.true_heading} deg")
|
|
|
|
def set_master_mode(self, mode: RadarMode):
|
|
"""API to change the Radar Master Mode."""
|
|
self.msg_a2.master_mode = mode
|
|
logger.info(f"Master Mode set to: {mode.name}")
|
|
|
|
def apply_startup_sequence(self, phase):
|
|
"""
|
|
Executes radar startup sequence based on C++ legacy behavior.
|
|
|
|
Args:
|
|
phase (int): Startup phase (1, 2, or 3)
|
|
1 = Protection mode (STANDBY=ON, SILENCE=ON) - send for 2-3 cycles
|
|
2 = Activation (STANDBY=OFF, SILENCE=OFF) - radar starts operating
|
|
3 = Operational (monitor transition_status in B7)
|
|
|
|
Returns:
|
|
dict: Status information for logging
|
|
"""
|
|
if phase == 1:
|
|
# Already configured in apply_defaults() - this is just for logging
|
|
logger.debug("[STARTUP] Phase 1: Protection mode active (STANDBY=ON, SILENCE=ON)")
|
|
return {
|
|
'phase': 1,
|
|
'standby': self.msg_a2.standby_cmd.name,
|
|
'silence': self.msg_a2.silence_reserved,
|
|
'description': 'Protection mode - radar safe'
|
|
}
|
|
|
|
elif phase == 2:
|
|
# CRITICAL: Disable STANDBY and SILENCE to activate radar
|
|
logger.info("[STARTUP] Phase 2: Activating radar (STANDBY→OFF, SILENCE→OFF)")
|
|
self.msg_a2.standby_cmd = StandbyStatus.OFF
|
|
self.msg_a2.silence_reserved = 0
|
|
logger.info(f" → Standby: {self.msg_a2.standby_cmd.name}")
|
|
logger.info(f" → Silence: {self.msg_a2.silence_reserved}")
|
|
logger.info(f" → Master Mode: {self.msg_a2.master_mode.name}")
|
|
logger.info(" → Expecting 6 cycles (~240ms @ 25Hz) for mode transition...")
|
|
return {
|
|
'phase': 2,
|
|
'standby': self.msg_a2.standby_cmd.name,
|
|
'silence': self.msg_a2.silence_reserved,
|
|
'description': 'Activation - radar starting',
|
|
'expected_cycles': 6
|
|
}
|
|
|
|
elif phase == 3:
|
|
logger.debug("[STARTUP] Phase 3: Monitoring transition completion...")
|
|
return {
|
|
'phase': 3,
|
|
'description': 'Operational - monitoring B7 tellback'
|
|
}
|
|
|
|
else:
|
|
logger.error(f"Invalid startup phase: {phase}")
|
|
return {'phase': 0, 'error': 'Invalid phase'}
|
|
|
|
def update_navigation_data(self, alt=None, tas=None):
|
|
"""Example API to update navigation data."""
|
|
if alt is not None:
|
|
self.msg_a4.baro_inertial_alt = alt
|
|
if tas is not None:
|
|
self.msg_a4.tas = tas |