diff --git a/dronecan_gui_tool/panels/serial_panel.py b/dronecan_gui_tool/panels/serial_panel.py index d80c3af..e0b00d2 100644 --- a/dronecan_gui_tool/panels/serial_panel.py +++ b/dronecan_gui_tool/panels/serial_panel.py @@ -10,13 +10,14 @@ from functools import partial from PyQt5.QtWidgets import QGridLayout, QWidget, QLabel, QDialog, \ QTableWidget, QVBoxLayout, QGroupBox, QTableWidgetItem, QLineEdit, \ - QComboBox, QHBoxLayout, QSpinBox + QComboBox, QHBoxLayout, QSpinBox, QCheckBox from PyQt5.QtCore import Qt, QTimer from ..widgets import get_icon from . import rtcm3 import time import socket import errno +import struct __all__ = 'PANEL_NAME', 'spawn', 'get_icon' @@ -24,6 +25,304 @@ _singleton = None +# protocol constants +PREAMBLE1 = 0xb5 +PREAMBLE2 = 0x62 + +CLASS_ACK = 0x05 +CLASS_CFG = 0x06 + +MSG_CFG_PRT = 0x00 + +MSG_ACK_NACK = 0x00 +MSG_ACK_ACK = 0x01 + +class UBloxError(Exception): + '''Ublox error class''' + def __init__(self, msg): + Exception.__init__(self, msg) + self.message = msg + +class UBloxDescriptor: + '''class used to describe the layout of a UBlox message''' + def __init__(self, name, msg_format, fields=[], count_field=None, format2=None, fields2=None): + self.name = name + self.msg_format = msg_format + self.fields = fields + self.count_field = count_field + self.format2 = format2 + self.fields2 = fields2 + + def ArrayParse(self, field): + '''parse an array descriptor''' + arridx = field.find('[') + if arridx == -1: + return (field, -1) + alen = int(field[arridx+1:-1]) + fieldname = field[:arridx] + return (fieldname, alen) + + def unpack(self, msg): + '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg''' + msg._fields = {} + + # unpack main message blocks. A comm + formats = self.msg_format.split(',') + buf = msg._buf[6:-2] + count = 0 + msg._recs = [] + fields = self.fields[:] + for fmt in formats: + size1 = struct.calcsize(fmt) + if size1 > len(buf): + raise UBloxError("%s INVALID_SIZE1: %u>%u fmt='%s'" % (self.name, size1, len(buf), fmt)) + f1 = list(struct.unpack(fmt, buf[:size1])) + i = 0 + while i < len(f1): + field = fields.pop(0) + (fieldname, alen) = self.ArrayParse(field) + if alen == -1: + msg._fields[fieldname] = f1[i] + if self.count_field == fieldname: + count = int(f1[i]) + i += 1 + else: + msg._fields[fieldname] = [0]*alen + for a in range(alen): + msg._fields[fieldname][a] = f1[i] + i += 1 + buf = buf[size1:] + if len(buf) == 0: + break + + if self.count_field == '_remaining': + count = len(buf) / struct.calcsize(self.format2) + + if count == 0: + msg._unpacked = True + #if len(buf) != 0: + # raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + return + + size2 = struct.calcsize(self.format2) + for c in range(count): + r = UBloxAttrDict() + if size2 > len(buf): + raise UBloxError("INVALID_SIZE=%u, " % len(buf)) + f2 = list(struct.unpack(self.format2, buf[:size2])) + for i in range(len(self.fields2)): + r[self.fields2[i]] = f2[i] + buf = buf[size2:] + msg._recs.append(r) + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + msg._unpacked = True + + def pack(self, msg, msg_class=None, msg_id=None): + '''pack a UBloxMessage from the .fields and ._recs attributes in msg''' + f1 = [] + if msg_class is None: + msg_class = msg.msg_class() + if msg_id is None: + msg_id = msg.msg_id() + msg._buf = '' + + fields = self.fields[:] + for f in fields: + (fieldname, alen) = self.ArrayParse(f) + if not fieldname in msg._fields: + break + if alen == -1: + f1.append(msg._fields[fieldname]) + else: + for a in range(alen): + f1.append(msg._fields[fieldname][a]) + try: + # try full length message + fmt = self.msg_format.replace(',', '') + msg._buf = struct.pack(fmt, *tuple(f1)) + except Exception as e: + # try without optional part + fmt = self.msg_format.split(',')[0] + msg._buf = struct.pack(fmt, *tuple(f1)) + + length = len(msg._buf) + if msg._recs: + length += len(msg._recs) * struct.calcsize(self.format2) + header = struct.pack('= level: + print(msg) + + def name(self): + '''return the short string name for a message''' + return "UBX(0x%02x,0x%02x,len=%u)" % (self.msg_class(), self.msg_id(), self.msg_length()) + + def msg_class(self): + '''return the message class''' + return self._buf[2] + + def msg_id(self): + '''return the message id within the class''' + return self._buf[3] + + def msg_type(self): + '''return the message type tuple (class, id)''' + return (self.msg_class(), self.msg_id()) + + def msg_length(self): + '''return the payload length''' + (payload_length,) = struct.unpack(' 0 and self._buf[0] != PREAMBLE1: + return False + if len(self._buf) > 1 and self._buf[1] != PREAMBLE2: + return False + needed = self.needed_bytes() + if needed > 1000: + return False + if needed == 0 and not self.valid(): + return False + return True + + def add(self, buf): + '''add some bytes to a message''' + self._buf += buf + while not self.valid_so_far() and len(self._buf) > 0: + '''handle corrupted streams''' + self._buf = self._buf[1:] + if self.needed_bytes() < 0: + self._buf = bytearray() + + def checksum(self, data=None): + '''return a checksum tuple for a message''' + if data is None: + data = self._buf[2:-2] + cs = 0 + ck_a = 0 + ck_b = 0 + for i in data: + ck_a = (ck_a + i) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return (ck_a, ck_b) + + def valid_checksum(self): + '''check if the checksum is OK''' + (ck_a, ck_b) = self.checksum() + d = self._buf[2:-2] + (ck_a2, ck_b2) = struct.unpack('= 8 and self.needed_bytes() == 0 and self.valid_checksum() + + def raw(self): + '''return the raw bytes''' + return self._buf + + def pack_message(self, msg_class, msg_id, payload): + self._buf = struct.pack('> " + msg_types[mtype].format(self.ublox_msg_out)) + return + except Exception as ex: + print(ex) + pass + print(">> " + self.ublox_msg_out.name()) + + def handle_ublox_data_out(self, buf): + '''handle ublox data from the GPS''' + if not self.ublox_handling.checkState(): + self.ublox_msg_out = None + return + if self.ublox_msg_out is None: + self.ublox_msg_out = UBloxMessage() + while len(buf) > 0: + needed = self.ublox_msg_out.needed_bytes() + n = min(needed, len(buf)) + self.ublox_msg_out.add(buf[:n]) + buf = buf[n:] + if self.ublox_msg_out.valid(): + self.handle_ublox_message_out() + self.ublox_msg_out = UBloxMessage() + + def handle_ublox_message_in(self): + '''handle a uBlox message from uCenter''' + mbuf = self.ublox_msg_in.raw() + mlen = len(mbuf) + + # avoid flooding the serial port by sleeping for a bit if we are + # getting data from uCenter faster than the serial port can handle it + port_rate = self.tunnel.baudrate / 10.0 + delay_needed = mlen / port_rate + time.sleep(delay_needed) + + # write message to the tunnel + self.tunnel.write(mbuf) + + # interpret the message to see if we should change baudrate + mtype = self.ublox_msg_in.msg_type() + if mtype in msg_types: + try: + msg_types[mtype].unpack(self.ublox_msg_in) + print("<< " + msg_types[mtype].format(self.ublox_msg_in)) + if mtype == (CLASS_CFG,MSG_CFG_PRT) and hasattr(self.ublox_msg_in,'baudRate'): + self.tunnel.baudrate = self.ublox_msg_in.baudRate + self.baud_select.setCurrentText("%u" % self.ublox_msg_in.baudRate) + print("uBlox changed baudrate to %u" % self.ublox_msg_in.baudRate) + return + except Exception as ex: + print(ex) + pass + print("<< " + self.ublox_msg_in.name()) + + def handle_ublox_data_in(self, buf): + '''handle ublox data from uCenter''' + if self.ublox_msg_in is None: + self.ublox_msg_in = UBloxMessage() + while len(buf) > 0: + needed = self.ublox_msg_in.needed_bytes() + n = min(needed, len(buf)) + self.ublox_msg_in.add(buf[:n]) + buf = buf[n:] + if self.ublox_msg_in.valid(): + self.handle_ublox_message_in() + self.ublox_msg_in = UBloxMessage() + def process_socket(self): '''process data from the socket''' while True: @@ -163,15 +554,20 @@ def process_socket(self): buf = self.sock.recv(120) except socket.error as ex: if ex.errno not in [ errno.EAGAIN, errno.EWOULDBLOCK ]: - print("Closing: ", ex) - self.sock = None - if self.tunnel is not None: - self.tunnel.close() - self.tunnel = None + self.close_socket() + return + except Exception: + self.close_socket() return + if buf is None or len(buf) == 0: break - self.tunnel.write(buf) + if self.ublox_handling.checkState(): + # we will send the data on packet boundaries + self.handle_ublox_data_in(buf) + else: + self.ublox_msg_in = None + self.tunnel.write(buf) self.num_tx_bytes += len(buf) self.tx_bytes.setText("%u" % self.num_tx_bytes) @@ -183,16 +579,24 @@ def process_tunnel(self): break try: self.sock.send(buf) - except Exception as ex: - print("Closing: ", ex) - self.sock = None - if self.tunnel is not None: - self.tunnel.close() - self.tunnel = None + self.handle_ublox_data_out(buf) + except Exception: + self.close_socket() return self.num_rx_bytes += len(buf) self.rx_bytes.setText("%u" % self.num_rx_bytes) + + def close_socket(self): + '''close the socket on errors''' + print("Closing socket") + self.state.setText("disconnected") + if self.sock is not None: + self.sock.close() + self.sock = None + if self.tunnel is not None: + self.tunnel.close() + self.tunnel = None def check_connection(self): '''called at 100Hz to process data''' diff --git a/dronecan_gui_tool/widgets/bus_monitor/window.py b/dronecan_gui_tool/widgets/bus_monitor/window.py index 3fd9942..6bf7727 100644 --- a/dronecan_gui_tool/widgets/bus_monitor/window.py +++ b/dronecan_gui_tool/widgets/bus_monitor/window.py @@ -338,7 +338,7 @@ def flip_row_mark(row, col): QTimer.singleShot(500, self._update_widget_sizes) def _update_widget_sizes(self): - max_footer_height = self.centralWidget().height() * 0.4 + max_footer_height = int(self.centralWidget().height() * 0.4) self._footer_splitter.setMaximumHeight(max_footer_height) def resizeEvent(self, qresizeevent):