150 lines
6.6 KiB
Python
150 lines
6.6 KiB
Python
from ..message_base import MessageBase
|
|
from ..constants import (
|
|
Subaddress, CursorMode, CursorSnowplough, CursorGhost, DTTEnable, SAREnable
|
|
)
|
|
from ..fields import BitField, EnumField, ScaledField
|
|
|
|
class MsgA4(MessageBase):
|
|
"""
|
|
Message A4: Navigation Data and Acquisition Cursor Data
|
|
|
|
ID: A4
|
|
Direction: BC -> RT (Receive)
|
|
Subaddress: 04
|
|
Rate: 50 Hz
|
|
Document Ref: 7.1.4
|
|
"""
|
|
SUBADDRESS = Subaddress.RX_NAV_DATA
|
|
IS_TRANSMIT = False
|
|
|
|
# --- Word 01: Validity and Slew Data (Ref 7.1.4.1) ---
|
|
nav_data_invalid = BitField(word_index=0, start_bit=0, width=1)
|
|
attitude_data_invalid = BitField(word_index=0, start_bit=1, width=1)
|
|
baro_inertial_alt_invalid = BitField(word_index=0, start_bit=2, width=1)
|
|
corr_baro_alt_invalid = BitField(word_index=0, start_bit=3, width=1)
|
|
radalt_invalid = BitField(word_index=0, start_bit=4, width=1)
|
|
spoi_alt_invalid = BitField(word_index=0, start_bit=5, width=1)
|
|
spoi_pos_invalid = BitField(word_index=0, start_bit=6, width=1)
|
|
tas_invalid = BitField(word_index=0, start_bit=7, width=1)
|
|
cas_invalid = BitField(word_index=0, start_bit=8, width=1)
|
|
ppos_invalid = BitField(word_index=0, start_bit=9, width=1)
|
|
antenna_slew_active = BitField(word_index=0, start_bit=10, width=1)
|
|
cursor_rates_invalid = BitField(word_index=0, start_bit=11, width=1)
|
|
|
|
cursor_mode = EnumField(word_index=0, start_bit=12, width=1, enum_cls=CursorMode)
|
|
cursor_zero_cmd = BitField(word_index=0, start_bit=13, width=1)
|
|
cursor_snowplough = EnumField(word_index=0, start_bit=14, width=1, enum_cls=CursorSnowplough)
|
|
# Bit 15 Spare
|
|
|
|
# --- Word 02: Time Tag (Ref 7.1.4.2) ---
|
|
# LSB = 64 us
|
|
time_tag = ScaledField(word_index=1, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Word 03: Present True Heading (Ref 7.1.4.3) ---
|
|
# Semicircles, 2's comp. Range +/- 1.0 (180 deg)
|
|
true_heading = ScaledField(word_index=2, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 04: Present Magnetic Heading (Ref 7.1.4.4) ---
|
|
mag_heading = ScaledField(word_index=3, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 05-07: Accelerations (Ref 7.1.4.5 - 7.1.4.7) ---
|
|
# LSB = 0.03125 ft/s^2, 2's comp
|
|
accel_x = ScaledField(word_index=4, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
accel_y = ScaledField(word_index=5, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
accel_z = ScaledField(word_index=6, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
|
|
# --- Word 08: True Air Speed (Ref 7.1.4.8) ---
|
|
# LSB = 0.125 knots, 2's comp
|
|
tas = ScaledField(word_index=7, start_bit=0, width=16, lsb_value=0.125, signed=True)
|
|
|
|
# --- Word 09: Calibrated Air Speed (Ref 7.1.4.9) ---
|
|
# LSB = 1.0 knots, Unsigned
|
|
cas = ScaledField(word_index=8, start_bit=0, width=16, lsb_value=1.0)
|
|
|
|
# --- Word 10: Barometric/Inertial Altitude (Ref 7.1.4.10) ---
|
|
# LSB = 4.0 ft, 2's comp
|
|
baro_inertial_alt = ScaledField(word_index=9, start_bit=0, width=16, lsb_value=4.0, signed=True)
|
|
|
|
# --- Word 11: Corrected Barometric Altitude (Ref 7.1.4.11) ---
|
|
# LSB = 4.0 ft, 2's comp
|
|
corr_baro_alt = ScaledField(word_index=10, start_bit=0, width=16, lsb_value=4.0, signed=True)
|
|
|
|
# --- Word 12: Radio Altimeter Altitude (Ref 7.1.4.12) ---
|
|
# LSB = 2.0 ft, Unsigned
|
|
radio_alt = ScaledField(word_index=11, start_bit=0, width=16, lsb_value=2.0)
|
|
|
|
# --- Word 13: SPOI Altitude (Ref 7.1.4.13) ---
|
|
# LSB = 1.0 ft, 2's comp
|
|
spoi_alt = ScaledField(word_index=12, start_bit=0, width=16, lsb_value=1.0, signed=True)
|
|
|
|
# --- Word 14: Clearance Plane Distance (Ref 7.1.4.14) ---
|
|
# LSB = 1.0 ft, Unsigned
|
|
clearance_plane = ScaledField(word_index=13, start_bit=0, width=16, lsb_value=1.0)
|
|
|
|
# --- Word 15: Wind Direction (Ref 7.1.4.15) ---
|
|
# Semicircles, 2's comp
|
|
wind_dir = ScaledField(word_index=14, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 16: Wind Speed (Ref 7.1.4.16) ---
|
|
# LSB = 0.0625 knots, Unsigned
|
|
wind_speed = ScaledField(word_index=15, start_bit=0, width=16, lsb_value=0.0625)
|
|
|
|
# --- Word 17-18: Antenna Demands (Ref 7.1.4.17 - 7.1.4.18) ---
|
|
# Semicircles, 2's comp
|
|
az_demand = ScaledField(word_index=16, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
el_demand = ScaledField(word_index=17, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 19: Cursor Rate X (Ref 7.1.4.19) ---
|
|
cursor_rate_x = ScaledField(word_index=18, start_bit=0, width=9, lsb_value=1.0, signed=True) # Pixels/sec
|
|
cursor_ghost = EnumField(word_index=18, start_bit=9, width=1, enum_cls=CursorGhost)
|
|
dtt_enable = EnumField(word_index=18, start_bit=10, width=1, enum_cls=DTTEnable)
|
|
sar_enable = EnumField(word_index=18, start_bit=11, width=1, enum_cls=SAREnable)
|
|
# Bits 12-15 Spare
|
|
|
|
# --- Word 20: Cursor Rate Y (Ref 7.1.4.20) ---
|
|
cursor_rate_y = ScaledField(word_index=19, start_bit=0, width=9, lsb_value=1.0, signed=True) # Pixels/sec
|
|
# Bits 9-15 Spare
|
|
|
|
# --- Word 21: Cursor Range (Ref 7.1.4.21) ---
|
|
# LSB = 8.0 ft, Unsigned
|
|
cursor_range = ScaledField(word_index=20, start_bit=0, width=16, lsb_value=8.0)
|
|
|
|
# --- Word 22: Cursor Azimuth (Ref 7.1.4.22) ---
|
|
# Semicircles, 2's comp
|
|
cursor_azimuth = ScaledField(word_index=21, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 23: Present Position Data Time Tag (Ref 7.1.4.23) ---
|
|
ppos_time_tag = ScaledField(word_index=22, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Helper for 25-bit Coordinates ---
|
|
def _get_25bit_semicircle(self, word_idx_msw, word_idx_lsw):
|
|
"""
|
|
Combines MSW (16 bit) and LSW (9 bit) to form a 25-bit 2's complement
|
|
Semicircle value. LSB = 5.96046e-8 (approx).
|
|
"""
|
|
msw = self._data[word_idx_msw]
|
|
lsw = (self._data[word_idx_lsw] >> 7) & 0x1FF
|
|
raw_25 = (msw << 9) | lsw
|
|
if raw_25 & (1 << 24):
|
|
raw_25 -= (1 << 25)
|
|
return float(raw_25 * 5.9604644775e-8)
|
|
|
|
# --- Word 24-25: Present Position Latitude (Ref 7.1.4.24) ---
|
|
@property
|
|
def ppos_lat(self):
|
|
return self._get_25bit_semicircle(23, 24)
|
|
|
|
# --- Word 26-27: Present Position Longitude (Ref 7.1.4.25) ---
|
|
@property
|
|
def ppos_lon(self):
|
|
return self._get_25bit_semicircle(25, 26)
|
|
|
|
# --- Word 28-29: SPOI Latitude (Ref 7.1.4.26) ---
|
|
@property
|
|
def spoi_lat(self):
|
|
return self._get_25bit_semicircle(27, 28)
|
|
|
|
# --- Word 30-31: SPOI Longitude (Ref 7.1.4.27) ---
|
|
@property
|
|
def spoi_lon(self):
|
|
return self._get_25bit_semicircle(29, 30) |