107 lines
4.2 KiB
Python
107 lines
4.2 KiB
Python
from ..message_base import MessageBase
|
|
from ..constants import Subaddress
|
|
from ..fields import BitField, ScaledField
|
|
|
|
class MsgA5(MessageBase):
|
|
"""
|
|
Message A5: INU High Speed Vector
|
|
|
|
ID: A5
|
|
Direction: BC -> RT (Receive)
|
|
Subaddress: 05
|
|
Rate: 50 Hz
|
|
Document Ref: 7.1.5
|
|
"""
|
|
SUBADDRESS = Subaddress.RX_INU_HS
|
|
IS_TRANSMIT = False
|
|
RATE_HZ = 50
|
|
|
|
# --- Word 01: INU Mode Word (Ref 7.1.5.1) ---
|
|
# Note: ICD says "disregarded by the Radar", mapped as raw.
|
|
inu_mode_word = BitField(word_index=0, start_bit=0, width=16)
|
|
|
|
# --- Word 02: INU Control Word 2 (Time Tag) (Ref 7.1.5.2) ---
|
|
# LSB = 64 us
|
|
time_tag = ScaledField(word_index=1, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Helper for 32-bit 2's complement Scaled Fields ---
|
|
def _get_32bit_scaled(self, word_idx_msw, word_idx_lsw, lsb_value):
|
|
"""
|
|
Combines MSW and LSW to form a 32-bit 2's complement value.
|
|
"""
|
|
msw = self._data[word_idx_msw]
|
|
lsw = self._data[word_idx_lsw]
|
|
|
|
raw_32 = (msw << 16) | lsw
|
|
|
|
# Handle 2's complement for 32 bits
|
|
if raw_32 & (1 << 31):
|
|
raw_32 -= (1 << 32)
|
|
|
|
return float(raw_32 * lsb_value)
|
|
|
|
# --- Words 03-04: X Velocity (Ref 7.1.5.3) ---
|
|
# 32 bit, 2's comp. LSB = 3.81470e-6 ft/sec
|
|
@property
|
|
def velocity_x(self):
|
|
return self._get_32bit_scaled(2, 3, 3.81470e-6)
|
|
|
|
# --- Words 05-06: Y Velocity (Ref 7.1.5.4) ---
|
|
@property
|
|
def velocity_y(self):
|
|
return self._get_32bit_scaled(4, 5, 3.81470e-6)
|
|
|
|
# --- Words 07-08: Z Velocity (Ref 7.1.5.5) ---
|
|
@property
|
|
def velocity_z(self):
|
|
return self._get_32bit_scaled(6, 7, 3.81470e-6)
|
|
|
|
# --- Word 09: Platform Azimuth (Ref 7.1.5.6) ---
|
|
# Semicircles, 2's comp
|
|
platform_azimuth = ScaledField(word_index=8, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 10: Roll (Ref 7.1.5.7) ---
|
|
roll = ScaledField(word_index=9, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 11: Pitch (Ref 7.1.5.8) ---
|
|
pitch = ScaledField(word_index=10, start_bit=0, width=16, lsb_value=3.05176e-5, signed=True)
|
|
|
|
# --- Word 12: Roll Rate (Ref 7.1.5.9) ---
|
|
# Units: Semicircles/sec. LSB = 1.22070e-4
|
|
roll_rate = ScaledField(word_index=11, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)
|
|
|
|
# --- Word 13: Pitch Rate (Ref 7.1.5.10) ---
|
|
pitch_rate = ScaledField(word_index=12, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)
|
|
|
|
# --- Word 14: Yaw Rate (Ref 7.1.5.11) ---
|
|
yaw_rate = ScaledField(word_index=13, start_bit=0, width=16, lsb_value=1.22070e-4, signed=True)
|
|
|
|
# --- Word 15: Longitudinal Acceleration (nx) (Ref 7.1.5.12) ---
|
|
# Units: ft/sec^2. LSB = 0.03125
|
|
accel_longitudinal = ScaledField(word_index=14, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
|
|
# --- Word 16: Lateral Acceleration (ny) (Ref 7.1.5.13) ---
|
|
accel_lateral = ScaledField(word_index=15, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
|
|
# --- Word 17: Normal Acceleration (nz) (Ref 7.1.5.14) ---
|
|
accel_normal = ScaledField(word_index=16, start_bit=0, width=16, lsb_value=0.03125, signed=True)
|
|
|
|
# --- Word 18: Platform Azimuth Time Tag (Ref 7.1.5.15) ---
|
|
# LSB = 64 us
|
|
time_tag_azimuth = ScaledField(word_index=17, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Word 19: Roll Time Tag (Ref 7.1.5.16) ---
|
|
time_tag_roll = ScaledField(word_index=18, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Word 20: Pitch Time Tag (Ref 7.1.5.17) ---
|
|
time_tag_pitch = ScaledField(word_index=19, start_bit=0, width=16, lsb_value=64.0)
|
|
|
|
# --- Word 21: Roll Axis Angular Acceleration (Ref 7.1.5.18) ---
|
|
# Units: Semicircles/sec^2. LSB = 2.44141e-4
|
|
ang_accel_roll = ScaledField(word_index=20, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True)
|
|
|
|
# --- Word 22: Pitch Axis Angular Acceleration (Ref 7.1.5.19) ---
|
|
ang_accel_pitch = ScaledField(word_index=21, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True)
|
|
|
|
# --- Word 23: Yaw Axis Angular Acceleration (Ref 7.1.5.20) ---
|
|
ang_accel_yaw = ScaledField(word_index=22, start_bit=0, width=16, lsb_value=2.44141e-4, signed=True) |