def test_change_filter(self): uut = UbxParser(UbxCID(0x00, 0x02)) uut.set_filter(UbxCID(0x13, 0x40)) uut.process(self.FRAME_1) cid, packet = uut.packet() assert cid == UbxCID(0x13, 0x40) uut.set_filter(UbxCID(0x00, 0x00)) uut.process(self.FRAME_1) cid, packet = uut.packet() assert cid is None and packet is None
def test_multiple_filters(self): uut = UbxParser(UbxCID(0x00, 0x02)) cids = [ UbxCID(0x12, 0x12), UbxCID(0x13, 0x40), UbxCID(0xFF, 0x00), UbxCID(0xFF, 0x00) ] uut.set_filters(cids) uut.process(self.FRAME_1) cid, packet = uut.packet() assert cid == UbxCID(0x13, 0x40)
def test_cinvalid_length(self): frame = [ 0xB5, 0x62, 0x13, 0x40, 0xe9, 0x03, 0x10, 0x00, 0x00, 0x12, 0xE4, 0x07, 0x09, 0x05, 0x06, 0x28, 0x30, 0x00, 0x40, 0x28, 0xEF, 0x0C, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0xAC ] uut = UbxParser(UbxCID(0x00, 0x02)) uut.set_filter(UbxCID(0x13, 0x41)) uut.process(frame) # CRC packet cid, packet = uut.packet( ) # Should be None because frame is too long (MAX_MESSAGE_LENGTH) assert cid is None and packet is None
def test_crc_error(self): # B5 62 13 40 18 00 10 00 00 12 E4 07 09 05 06 28 30 00 40 28 EF 0C 0A 00 00 00 00 00 00 00 51 AC # hdr | <-- checksum --> | chksum frame = [ 0xB5, 0x62, 0x13, 0x40, 0x18, 0x00, 0x10, 0x00, 0x00, 0x12, 0xE4, 0x07, 0x09, 0x05, 0x06, 0x28, 0x30, 0x00, 0x40, 0x28, 0xEF, 0x0C, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0xAC + 1 ] uut = UbxParser(UbxCID(0x00, 0x02)) uut.set_filter(UbxCID(0x13, 0x41)) uut.process(frame) # CRC packet cid, packet = uut.packet() assert cid == UbxCID(0x00, 0x02)
def test_dropped_id(self): uut = UbxParser(UbxCID(0x00, 0x02)) uut.set_filter(UbxCID(0x13, 0x41)) uut.process(self.FRAME_1) cid, packet = uut.packet() assert cid is None and packet is None
def test_passes_filter(self): uut = UbxParser(UbxCID(0x00, 0x02)) uut.set_filter(UbxCID(0x13, 0x40)) uut.process(self.FRAME_1) cid, packet = uut.packet() assert cid == UbxCID(0x13, 0x40)
def test_no_frames(self): uut = UbxParser(UbxCID(0x00, 0x02)) packet = uut.packet() assert packet == (None, None)
class UbxServerBase_(object): def __init__(self): super().__init__() self.cid_crc_error = UbxCID(0x00, 0x02) self.parser = UbxParser(self.cid_crc_error) self.frame_factory = FrameFactory.getInstance() self.max_retries = 2 self.retry_delay_in_ms = 1800 def setup(self): # Register ACK-ACK/ACK-NAK frames, as they are used internally by this module self.frame_factory.register(UbxAckAck) self.frame_factory.register(UbxAckNak) # Register MGA-ACK-DATA0 so we can check result of MGA requests self.frame_factory.register(UbxMgaAckData0) return True def cleanup(self): self.frame_factory.destroy() self.frame_factory = None def register_frame(self, frame_type): self.frame_factory.register(frame_type) def set_retries(self, retries): logger.debug(f"setting max retries to {retries}") assert 0 <= retries <= 10 current = self.max_retries self.max_retries = retries return current def set_retry_delay(self, delay): logger.debug(f"setting retry delay to {delay} ms") assert 0 <= delay <= 5000 current = self.retry_delay_in_ms self.retry_delay_in_ms = delay return current def poll(self, frame_poll): """ Poll a receiver status - sends the poll message - waits for receiver message with same class/id as poll message - if poll is for configuration frame - wait for ACK (or NAK) to poll request (note: ACK is sent after response) - retries in case no answer is received """ assert isinstance(frame_poll, UbxFrame) logger.debug(f"polling {frame_poll.NAME}") # In general we expect a response frame with the exact same CID. # If this is a configuration message, also check ACK frame, if frame_poll.CID.cls == UbxCID.CLASS_CFG: wait_cids = [frame_poll.CID, UbxAckAck.CID, UbxAckNak.CID] else: wait_cids = [frame_poll.CID] self.parser.set_filters(wait_cids) # Serialize polling frame payload. # Only a few polling frames required payload, most come w/o. frame_poll.pack() for retry in range(self.max_retries + 1): if retry != 0: logger.warning(f'poll: timeout, retrying {retry}') self._flush_input() res = self._send(frame_poll) if res: state = 'wait-response' response = None t_start = time.time() self.parser.empty_queue() self.parser.restart() while state != "ok" and state != 'timeout': packet = self._wait() if packet: t_duration = time.time() - t_start if state == 'wait-response': check = self._check_poll(frame_poll, packet) if check: logger.debug( f'response received after {t_duration:.2f} s' ) response = packet if frame_poll.CID.cls == UbxCID.CLASS_CFG: # Only wait for ack if this is a CFG request .. state = 'wait-ack' else: # .. otherwise we are done here state = 'ok' elif state == 'wait-ack': check = self._check_ack_nak(frame_poll, packet) if check == 'ACK': logger.debug( f'ACK received after {t_duration:.2f} s') state = 'ok' elif check == 'NAK': # Not sure why we should ever get a NAK frame logger.warning(f"NAK received\n{packet}") else: state = 'timeout' break if state == 'ok': assert response return response else: self._recover() else: logger.warning('poll: send failed') def set(self, frame_set): """ Send a set message to modem and wait for acknowledge - creates bytes representation of set frame - sends set message to modem - waits for ACK/NAK """ assert isinstance(frame_set, UbxFrame) logger.debug(f"setting {frame_set.NAME}") # Wait for ACK-ACK / ACK-NAK self.parser.set_filters([UbxAckAck.CID, UbxAckNak.CID]) # Get frame data (header, cls, id, len, payload, checksum a/b) frame_set.pack() for retry in range(self.max_retries + 1): if retry != 0: logger.warning(f'set: timeout, retrying {retry}') self._flush_input() res = self._send(frame_set) if res: t_start = time.time() self.parser.empty_queue() self.parser.restart() packet = self._wait() if packet: t_duration = time.time() - t_start check = self._check_ack_nak(frame_set, packet) if check == 'ACK': logger.debug(f'ACK received after {t_duration:.2f} s') return packet elif check == 'NAK': logger.debug(f'NAK received after {t_duration:.2f} s') return packet else: self._recover() else: logger.warning('set: send failed') def set_mga(self, frame_set_mga): """ Send an MGA set message to modem and wait for acknowledge Note: MGA acknowledge must be enabled via CFG-NAVX5 to use this method. - creates bytes representation of set frame - sends set message to modem - waits for ACK-MGA-DATA0 """ assert isinstance(frame_set_mga, UbxFrame) assert frame_set_mga.CID.cls == 0x13 # TODO: Not best style to hardcode 0x13 for MGA Class logger.debug(f"setting mga {frame_set_mga.NAME}") # Wait for special MGA ACK frame self.parser.set_filter(UbxMgaAckData0.CID) # Get frame data (header, cls, id, len, payload, checksum a/b) frame_set_mga.pack() for retry in range(self.max_retries + 1): if retry != 0: logger.warning(f'set_mga: timeout, retrying {retry}') self._flush_input() res = self._send(frame_set_mga) if res: t_start = time.time() self.parser.empty_queue() self.parser.restart() packet = self._wait() if packet: t_duration = time.time() - t_start check = self._check_mga(frame_set_mga, packet) if check: logger.debug( f'MGA-ACK received after {t_duration:.2f} s') return packet else: self._recover() else: logger.warning('set_mga: send failed') def fire_and_forget(self, frame_set): """ Send a set message to modem without waiting for a response (fire and forget) This method is typically used for commands that are not ACKed, i.e. - cold start - change baudrate """ assert isinstance(frame_set, UbxFrame) logger.debug(f"firing {frame_set.NAME}") frame_set.pack() self._send(frame_set) @DeprecationWarning def send(self, message): self.fire_and_forget(message) """ Overrides to be provided by backend implementation """ def _recover(self): """ Perform actions required to recover communication link. Occassionaly the communication fails with lots of checksum errors. The reason why erraneous frames are suddenly received is not clear. Usually one byte in a frame is just wrong, no bytes are missing or duplicated. Possible problems - Overload of receiver - UART driver problem <- likely - Physical interface problem Must be implemented by derived class. - i.e. close and reopen serial tty. """ raise NotImplementedError def _receive(self): """ This method must be implemented by a derived backend implementation It shall try to receive data from the modem. Any data received shall be returned. In case no data could be read, return None. Maximum runtime of this function is expected to be 100 ms so that incoming data is moved to processing stage quickly. """ raise NotImplementedError def _transmit(self, data): """ This method must be implemented by a derived backend implementation It shall send the frame to the modem. Typical operation is to convert the frame to a uxb message with UbxFrame::to_bytes(). Then send the byte buffer using the backend device In case of success True shall be returned, False otherwise """ raise NotImplementedError def _flush_input(self): """ This method can be implemented by a derived backend If implemented, it shall flush all pending data in the input/receive queue. """ pass """ Private methods """ def _send(self, ubx_message): """ Send ubx frame to modem via backend driver """ if logger.isEnabledFor(logging.DEBUG): logger.debug(f'sending {ubx_message}') msg_in_binary = ubx_message.to_bytes() res = self._transmit(msg_in_binary) if not res: logger.warning('command could not be sent') return res def _wait(self): retry_delay_in_s = self.retry_delay_in_ms / 1000.0 if logger.isEnabledFor(logging.DEBUG): logger.debug(f'waiting {retry_delay_in_s}s for response') # self.parser.empty_queue() # self.parser.restart() time_end = time.time() + retry_delay_in_s while time.time() < time_end: data = self._receive() if data: # process() places all decoded frames in rx queue self.parser.process(data) # Check if process could decode one or more frames # Loop exists when no more frames are to handle cid, data = self.parser.packet() if cid: if cid != self.cid_crc_error: if logger.isEnabledFor(logging.DEBUG): logger.debug(f'received expected frame {cid}') ff = FrameFactory.getInstance() try: frame = ff.build_with_data(cid, data) return frame except KeyError: # We can't parse the frame, is it registered() logger.warning( f'frame not registered, cannot decode: {binascii.hexlify(data)}' ) else: logger.warning("checksum error in frame, discarding") logger.warning('timeout...') def _check_poll(self, request, res): """ Check if response is for requested frame """ if res.CID == request.CID: # if logger.isEnabledFor(logging.DEBUG): # logger.debug('response matches request') return True else: # Must never happen, as only request CID is in expected list logger.warning(f'invalid frame received {res.CID}') def _check_ack_nak(self, request, res): if res.CID == UbxAckAck.CID: ack_cid = UbxCID(res.f.clsId, res.f.msgId) if ack_cid == request.CID: # if logger.isEnabledFor(logging.DEBUG): # logger.debug('ACK matches request') return "ACK" else: # ACK is for another request logger.warning( f'ACK {ack_cid} does not match request {request.CID}') elif res.CID == UbxAckNak.CID: logger.warning(f'request {request.CID} rejected, NAK received') return "NAK" else: # Must never happen. Only ACK/NAK in expected list logger.error(f'invalid frame received {res.CID}') def _check_mga(self, request, res): if res.CID == UbxMgaAckData0.CID: if res.f.type == 1: return True else: logger.warning( f'MGA message not accepted, error code {res.f.infoCode}') else: # Must never happen. Only MGA ACK in expected list logger.error(f'invalid frame received {res.CID}')