From c56cc7e218f25d0e34f0ef306d2d500083caddd9 Mon Sep 17 00:00:00 2001 From: giumas Date: Sun, 4 Feb 2024 09:03:49 +0100 Subject: [PATCH] added support for the SSM datagram --- .../gui/soundspeedsettings/widgets/input.py | 46 +++++++------- .../soundspeedsettings/widgets/listeners.py | 44 ++++++------- hyo2/ssm2/lib/formats/kmall.py | 57 ++++++++++++++++- hyo2/ssm2/lib/listener/sis/sis.py | 63 +++++++++++++++++-- 4 files changed, 157 insertions(+), 53 deletions(-) diff --git a/hyo2/ssm2/app/gui/soundspeedsettings/widgets/input.py b/hyo2/ssm2/app/gui/soundspeedsettings/widgets/input.py index a489d844..3902bff4 100644 --- a/hyo2/ssm2/app/gui/soundspeedsettings/widgets/input.py +++ b/hyo2/ssm2/app/gui/soundspeedsettings/widgets/input.py @@ -265,14 +265,14 @@ def __init__(self, main_win, db): vbox.addWidget(self.use_sis5) vbox.addStretch() - # - use sippican + # - use nmea hbox = QtWidgets.QHBoxLayout() self.right_layout.addLayout(hbox) # -- label vbox = QtWidgets.QVBoxLayout() hbox.addLayout(vbox) vbox.addStretch() - label = QtWidgets.QLabel("Listen Sippican:") + label = QtWidgets.QLabel("Listen NMEA 0183:") label.setFixedWidth(lbl_width) vbox.addWidget(label) vbox.addStretch() @@ -280,19 +280,19 @@ def __init__(self, main_win, db): vbox = QtWidgets.QVBoxLayout() hbox.addLayout(vbox) vbox.addStretch() - self.use_sippican = QtWidgets.QComboBox() - self.use_sippican.addItems(["True", "False"]) - vbox.addWidget(self.use_sippican) + self.use_nmea_0183 = QtWidgets.QComboBox() + self.use_nmea_0183.addItems(["True", "False"]) + vbox.addWidget(self.use_nmea_0183) vbox.addStretch() - # - use nmea + # - use sippican hbox = QtWidgets.QHBoxLayout() self.right_layout.addLayout(hbox) # -- label vbox = QtWidgets.QVBoxLayout() hbox.addLayout(vbox) vbox.addStretch() - label = QtWidgets.QLabel("Listen NMEA 0183:") + label = QtWidgets.QLabel("Listen Sippican:") label.setFixedWidth(lbl_width) vbox.addWidget(label) vbox.addStretch() @@ -300,10 +300,10 @@ def __init__(self, main_win, db): vbox = QtWidgets.QVBoxLayout() hbox.addLayout(vbox) vbox.addStretch() - self.use_nmea_0183 = QtWidgets.QComboBox() - self.use_nmea_0183.addItems(["True", "False"]) - vbox.addWidget(self.use_nmea_0183) - vbox.addStretch() + self.use_sippican = QtWidgets.QComboBox() + self.use_sippican.addItems(["True", "False"]) + vbox.addWidget(self.use_sippican) + vbox.addStretch() # - use mvp hbox = QtWidgets.QHBoxLayout() @@ -414,9 +414,9 @@ def __init__(self, main_win, db): # noinspection PyUnresolvedReferences self.use_sis5.currentIndexChanged.connect(self.apply_use_sis) # noinspection PyUnresolvedReferences - self.use_sippican.currentIndexChanged.connect(self.apply_use_sippican) + self.use_nmea_0183.currentIndexChanged.connect(self.apply_use_sis) # noinspection PyUnresolvedReferences - self.use_nmea_0183.currentIndexChanged.connect(self.apply_use_nmea_0183) + self.use_sippican.currentIndexChanged.connect(self.apply_use_sippican) # noinspection PyUnresolvedReferences self.use_mvp.currentIndexChanged.connect(self.apply_use_mvp) # noinspection PyUnresolvedReferences @@ -495,33 +495,29 @@ def apply_use_sis(self): self.db.use_sis4 = use_value if use_value: self.use_sis5.setCurrentText("False") + self.use_nmea_0183.setCurrentText("False") elif self.sender() is self.use_sis5: use_value = self.use_sis5.currentText() == "True" self.db.use_sis5 = use_value if use_value: self.use_sis4.setCurrentText("False") + self.use_nmea_0183.setCurrentText("False") + elif self.sender() is self.use_nmea_0183: + use_value = self.use_nmea_0183.currentText() == "True" + self.db.use_nmea_0183 = use_value + if use_value: + self.use_sis4.setCurrentText("False") + self.use_sis5.setCurrentText("False") self.setup_changed() self.main_win.reload_settings() - def apply_use_sis5(self): - # logger.debug("apply use SIS") - self.db.use_sis5 = self.use_sis5.currentText() == "True" - self.setup_changed() - self.main_win.reload_settings() - def apply_use_sippican(self): # logger.debug("apply use Sippican") self.db.use_sippican = self.use_sippican.currentText() == "True" self.setup_changed() self.main_win.reload_settings() - def apply_use_nmea_0183(self): - # logger.debug("apply use Nmea") - self.db.use_nmea_0183 = self.use_nmea_0183.currentText() == "True" - self.setup_changed() - self.main_win.reload_settings() - def apply_use_mvp(self): # logger.debug("apply use MVP") self.db.use_mvp = self.use_mvp.currentText() == "True" diff --git a/hyo2/ssm2/app/gui/soundspeedsettings/widgets/listeners.py b/hyo2/ssm2/app/gui/soundspeedsettings/widgets/listeners.py index abd99d30..da3fc8ac 100644 --- a/hyo2/ssm2/app/gui/soundspeedsettings/widgets/listeners.py +++ b/hyo2/ssm2/app/gui/soundspeedsettings/widgets/listeners.py @@ -81,15 +81,15 @@ def __init__(self, main_win, db): self.left_layout.addSpacing(12) - # SIPPICAN + # NMEA hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) hbox.addStretch() - self.label = QtWidgets.QLabel("Sippican(*):") + self.label = QtWidgets.QLabel("NMEA 0183(*):") hbox.addWidget(self.label) - hbox.addStretch() + hbox.addStretch() - # - sippican_listen_port + # - nmea_listen_port hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) # -- label @@ -97,12 +97,12 @@ def __init__(self, main_win, db): label.setFixedWidth(lbl_width) hbox.addWidget(label) # -- value - self.sippican_listen_port = QtWidgets.QLineEdit() + self.nmea_listen_port = QtWidgets.QLineEdit() validator = QtGui.QIntValidator(0, 99999) - self.sippican_listen_port.setValidator(validator) - hbox.addWidget(self.sippican_listen_port) + self.nmea_listen_port.setValidator(validator) + hbox.addWidget(self.nmea_listen_port) - # - sippican_listen_timeout + # - nmea_listen_timeout hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) # -- label @@ -110,22 +110,22 @@ def __init__(self, main_win, db): label.setFixedWidth(lbl_width) hbox.addWidget(label) # -- value - self.sippican_listen_timeout = QtWidgets.QLineEdit() + self.nmea_listen_timeout = QtWidgets.QLineEdit() validator = QtGui.QIntValidator(0, 99999) - self.sippican_listen_timeout.setValidator(validator) - hbox.addWidget(self.sippican_listen_timeout) + self.nmea_listen_timeout.setValidator(validator) + hbox.addWidget(self.nmea_listen_timeout) self.left_layout.addSpacing(12) - # NMEA + # SIPPICAN hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) hbox.addStretch() - self.label = QtWidgets.QLabel("NMEA 0183(*):") + self.label = QtWidgets.QLabel("Sippican(*):") hbox.addWidget(self.label) - hbox.addStretch() + hbox.addStretch() - # - nmea_listen_port + # - sippican_listen_port hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) # -- label @@ -133,12 +133,12 @@ def __init__(self, main_win, db): label.setFixedWidth(lbl_width) hbox.addWidget(label) # -- value - self.nmea_listen_port = QtWidgets.QLineEdit() + self.sippican_listen_port = QtWidgets.QLineEdit() validator = QtGui.QIntValidator(0, 99999) - self.nmea_listen_port.setValidator(validator) - hbox.addWidget(self.nmea_listen_port) + self.sippican_listen_port.setValidator(validator) + hbox.addWidget(self.sippican_listen_port) - # - nmea_listen_timeout + # - sippican_listen_timeout hbox = QtWidgets.QHBoxLayout() self.left_layout.addLayout(hbox) # -- label @@ -146,10 +146,10 @@ def __init__(self, main_win, db): label.setFixedWidth(lbl_width) hbox.addWidget(label) # -- value - self.nmea_listen_timeout = QtWidgets.QLineEdit() + self.sippican_listen_timeout = QtWidgets.QLineEdit() validator = QtGui.QIntValidator(0, 99999) - self.nmea_listen_timeout.setValidator(validator) - hbox.addWidget(self.nmea_listen_timeout) + self.sippican_listen_timeout.setValidator(validator) + hbox.addWidget(self.sippican_listen_timeout) self.left_layout.addStretch() diff --git a/hyo2/ssm2/lib/formats/kmall.py b/hyo2/ssm2/lib/formats/kmall.py index 46071280..2d77f3c4 100644 --- a/hyo2/ssm2/lib/formats/kmall.py +++ b/hyo2/ssm2/lib/formats/kmall.py @@ -25,6 +25,8 @@ class Kmall: b'#SDE': 'Sensor (S) data from depth (DE) sensor', b'#SHI': 'Sensor (S) data for height (HI)', + b'#SSM': 'Sound Speed Manager (SSM) datagram', + b'#CPO': 'Compatibility (C) data for position (PO)', b'#CHE': 'Compatibility (C) data for heave (HE)', } @@ -76,6 +78,57 @@ def __str__(self): % (self.id, self.dg_time, self.sounder_id) +class KmallSSM(Kmall): + def __init__(self, data, debug: bool = False): + super().__init__(data) + + common = struct.unpack("<4H", self.data[20:28]) + # common_length = common[0] + # logger.debug("common part -> length: %d/%d" % (common_length, self.length)) + # common_sensor_system = common[1] + # logger.debug("common part -> sensor system: %d" % common_sensor_system) + common_sensor_status = common[2] + self.inactive_sensor = (common_sensor_status & 1) != 1 + self.invalid_data = (common_sensor_status & 16) == 16 + # if self.inactive_sensor or self.invalid_data: + # logger.debug("common part -> sensor status: %s (inactive: %s, invalid: %s)" + # % (common_sensor_status, self.inactive_sensor, self.invalid_data)) + + data_blk = struct.unpack("<2fIfI2d", self.data[28:64]) + self.tss = data_blk[0] + # logger.debug('TSS: %s m/s' % (self.tss, )) + self.transducer_depth = data_blk[1] + # logger.debug('transducer depth re WL: %.3f m' % (self.transducer_depth)) + # ping_time_sec = data_blk[2] + # logger.debug('ping time sec: %s' % ping_time_sec) + # ping_datetime = Kmall.kmall_datetime(ping_time_sec, 0) + # logger.debug('ping datetime: %s' % ping_datetime.strftime('%Y-%m-%d %H:%M:%S.%f')) + self.mean_depth = data_blk[3] + # logger.debug('ping avg depth: %s' % self.mean_depth) + # nav_time_sec = data_blk[4] + # logger.debug('nav time sec: %s' % nav_time_sec) + # nav_datetime = Kmall.kmall_datetime(nav_time_sec, 0) + # logger.debug('nav datetime: %s' % nav_datetime.strftime('%Y-%m-%d %H:%M:%S.%f')) + self.latitude = data_blk[5] + self.longitude = data_blk[6] + if debug: + logger.debug('SSM -> pos: %.7f, %.7f' % (self.latitude, self.longitude)) + + final_length = struct.unpack(" Invalid length: %s != %s' % (final_length, self.length)) + + def __str__(self): + output = Kmall.__str__(self) + output += '\tTSS: %.2f m/s\n\tTransducer depth: %.2f m.\n' % (self.tss, self.transducer_depth) + output += '\tLat/Lon: %lf %lf\n' % (self.latitude, self.longitude) + output += '\tMean depth: %.2f m\n' % self.mean_depth + + return output + + class KmallMRZ(Kmall): def __init__(self, data, debug: bool = False): super().__init__(data) @@ -117,7 +170,7 @@ def __init__(self, data, debug: bool = False): self.tss = ping_info[36] # logger.debug('TSS: %s m/s' % (self.tss, )) transducer_depth_m = ping_info[37] - z_water_level_re_ref_point_m = ping_info[38] + # z_water_level_re_ref_point_m = ping_info[38] # logger.debug('WL re RP: %.3f m, transducer depth re WL: %.3f m' # % (z_water_level_re_ref_point_m, transducer_depth_m)) self.transducer_depth = transducer_depth_m @@ -155,7 +208,7 @@ def __init__(self, data, debug: bool = False): # % (i, len(self.data[start_sounding:end_sounding]))) break sounding = struct.unpack(sounding_struct, self.data[start_sounding:end_sounding]) - sounding_idx = sounding[0] + # sounding_idx = sounding[0] # logger.debug("sounding -> #%d" % (sounding_idx, )) sounding_detection_type = sounding[2] # logger.debug("sounding -> detection type: %s" % (sounding_detection_type, )) diff --git a/hyo2/ssm2/lib/listener/sis/sis.py b/hyo2/ssm2/lib/listener/sis/sis.py index b488a9a0..9afb11fc 100644 --- a/hyo2/ssm2/lib/listener/sis/sis.py +++ b/hyo2/ssm2/lib/listener/sis/sis.py @@ -52,7 +52,9 @@ def __init__(self): class Sis5: def __init__(self): - self.datagrams = [b'#MRZ', b'#SPO', b'#SVP'] + self.datagrams = [b'#SSM', b'#MRZ', b'#SPO', b'#SVP'] + self.ssm = None # type: Optional[kmall.KmallSSM] + self.ssm_count = 0 self.mrz = None # type: Optional[kmall.KmallMRZ] self.mrz_count = 0 self.spo = None # type: Optional[kmall.KmallSPO] @@ -98,27 +100,33 @@ def clear_ssp(self) -> None: self.sis4.ssp = None @property - def nav(self) -> Union[km.KmNav, kmall.KmallSPO]: + def nav(self) -> Union[km.KmNav, kmall.KmallSPO, kmall.KmallSSM]: if self.use_sis5: + if self.sis5.ssm: + return self.sis5.ssm return self.sis5.spo else: return self.sis4.nav def clear_nav(self) -> None: if self.use_sis5: + self.sis5.ssm = None self.sis5.spo = None else: self.sis4.nav = None @property - def xyz(self) -> Union[km.KmXyz88, kmall.KmallMRZ]: + def xyz(self) -> Union[km.KmXyz88, kmall.KmallMRZ, kmall.KmallSSM]: if self.use_sis5: + if self.sis5.ssm: + return self.sis5.ssm return self.sis5.mrz else: return self.sis4.xyz88 def clear_xyz(self) -> None: if self.use_sis5: + self.sis5.ssm = None self.sis5.mrz = None else: self.sis4.xyz88 = None @@ -126,10 +134,16 @@ def clear_xyz(self) -> None: @property def xyz_transducer_depth(self) -> Optional[float]: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.transducer_depth + if self.sis5.mrz is None: return None return self.sis5.mrz.transducer_depth + else: + if self.sis4.xyz88 is None: return None return self.sis4.xyz88.transducer_draft @@ -137,10 +151,16 @@ def xyz_transducer_depth(self) -> Optional[float]: @property def xyz_transducer_sound_speed(self) -> Optional[float]: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.tss + if self.sis5.mrz is None: return None return self.sis5.mrz.tss + else: + if self.sis4.xyz88 is None: return None return self.sis4.xyz88.sound_speed @@ -148,17 +168,28 @@ def xyz_transducer_sound_speed(self) -> Optional[float]: @property def xyz_mean_depth(self) -> float: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.mean_depth + return self.sis5.mrz.mean_depth + else: return self.sis4.xyz88.mean_depth @property def nav_latitude(self) -> Optional[float]: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.latitude + if self.sis5.spo is None: return None return self.sis5.spo.latitude + else: + if self.sis4.nav is None: return None return self.sis4.nav.latitude @@ -166,10 +197,16 @@ def nav_latitude(self) -> Optional[float]: @property def nav_longitude(self) -> Optional[float]: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.longitude + if self.sis5.spo is None: return None return self.sis5.spo.longitude + else: + if self.sis4.nav is None: return None return self.sis4.nav.longitude @@ -177,10 +214,16 @@ def nav_longitude(self) -> Optional[float]: @property def nav_timestamp(self) -> Optional[float]: if self.use_sis5: + + if self.sis5.ssm: + return self.sis5.ssm.dg_time + if self.sis5.spo is None: return None return self.sis5.spo.dg_time + else: + if self.sis4.nav is None: return None return self.sis4.nav.dg_time @@ -298,7 +341,18 @@ def _parse_sis5(self) -> None: logger.debug("Ignoring received datagram") return - if self.cur_id == b'#MRZ': + if self.cur_id == b'#SSM': + ssm = kmall.KmallSSM(this_data, self.debug) + if ssm.invalid_data or ssm.inactive_sensor: + return + self.sis5.ssm = ssm + self.sis5.ssm_count += 1 + self.nav_last_time = datetime.utcnow() + self.xyz_last_time = datetime.utcnow() + if self.debug: + logger.debug("Parsed") + + elif self.cur_id == b'#MRZ': partition = struct.unpack("<2H", this_data[20:24]) nr_of_datagrams = partition[0] datagram_nr = partition[1] @@ -405,6 +459,7 @@ def send_profile(self, ssp: ProfileList, ip: str, port: int) -> None: def info(self) -> str: msg = "Received datagrams:\n" if self.use_sis5: + msg += "- SSM: %d\n" % self.sis5.ssm_count msg += "- MRZ: %d\n" % self.sis5.mrz_count msg += "- SPO: %d\n" % self.sis5.spo_count msg += "- SVP: %d\n" % self.sis5.svp_count