class FrameDecoder: INVALID_FRAME = object() def __init__(self): self._frame = Frame() self._decoded = WriteBuffer(buffer=self._frame.buffer) self._checksum_total = 0 self._escaping = False def reset(self): self._decoded.reset_offset() self._checksum_total = 0 self._escaping = False def decode(self, b): if b == Code.ESCAPE: self._escaping = True return None elif self._escaping: self._escaping = False b ^= Code.ESCAPE_XOR self._checksum_total += b if self._decoded.has_remaining(): self._decoded.write_u8(b) return None # We've received the complete frame so validate and return it. if not Checksum.validate(self._checksum_total): _logger.error("invalid checksum") return self.INVALID_FRAME return self._frame
def __init__(self): self._frame_payload = ReadBuffer() self._request = WriteBuffer(length=self._BUFFER_LEN) self._result = MspRequestResult() self._command = -1 self._started = False self._last_seq = -1
def __init__(self): self._sequence = loop(0x10) self._command = 0 self._is_error = False self._response = ReadBuffer() self._error_buffer = memoryview(bytearray(1)) self._frame_payload = WriteBuffer()
class FrameEncoder: _BUFFER_LEN = 16 # Worst case: frame ID + payload + checksum = 8 and every byte is doubled by escaping. def __init__(self): self._frame = Frame() self._encoded = WriteBuffer(length=self._BUFFER_LEN) def get_frame(self): return self._frame def _append(self, b): if b == Code.ESCAPE or b == Code.START: self._encoded.write_u8(Code.ESCAPE) b ^= Code.ESCAPE_XOR self._encoded.write_u8(b) def encode(self): self._encoded.reset_offset() for b in self._frame.buffer: self._append(b) total = sum(self._frame.buffer) self._append(Checksum.calculate(total)) return self._encoded.get_buffer()
class MspResponseEncoder: _BUFFER_LEN = 64 # TODO: see how long responses typically are. def __init__(self): self._sequence = loop(0x10) self._command = 0 self._is_error = False self._response = ReadBuffer() self._error_buffer = memoryview(bytearray(1)) self._frame_payload = WriteBuffer() @staticmethod def create_response_buffer(): return WriteBuffer(length=MspResponseEncoder._BUFFER_LEN) def _reset(self, command, buffer, is_error): self._command = command self._is_error = is_error self._response.set_buffer(buffer) def set_error(self, error, command): self._error_buffer[0] = error self._reset(command, self._error_buffer, is_error=True) def set_command(self, command, buffer): self._reset(command, buffer, is_error=False) def encode(self, frame): frame.set_id(FrameId.MSP_SERVER) self._frame_payload.set_buffer(frame.payload) header = next(self._sequence) response_remaining = self._response.remaining() # Write the start header if this is the frame of a given response. if self._response.get_offset() == 0: # Unlike the request, there's no version included in the header byte. # And the command isn't included as the third byte (but it is factored into the checksum). header |= MspHeaderBits.START_FLAG if self._is_error: header |= MspHeaderBits.ERROR_FLAG self._frame_payload.write_u8(header) self._frame_payload.write_u8(response_remaining) else: self._frame_payload.write_u8(header) frame_remaining = self._frame_payload.remaining() remaining = min(frame_remaining, response_remaining) self._frame_payload.write(self._response.read(remaining)) if response_remaining >= frame_remaining: return True checksum = self._calculate_checksum(self._command, self._response.get_buffer()) self._frame_payload.write_u8(checksum) # Pad out the rest of the frame - the value doesn't matter but 0 looks nicer when debugging. while self._frame_payload.has_remaining(): self._frame_payload.write_u8(0) return False # No more to come. @staticmethod def _calculate_checksum(command, buf): checksum = command ^ len(buf) for b in buf: checksum ^= b return checksum
def create_response_buffer(): return WriteBuffer(length=MspResponseEncoder._BUFFER_LEN)
def __init__(self): self._payload = WriteBuffer(length=self._BUFFER_LEN) self._frame = SbusFrame() self._searching = True
class SbusDecoder: _START_BYTE = 0x0F _BUFFER_LEN = 23 _CH16_FLAG = 0x01 _CH17_FLAG = 0x02 _LOST_FRAME_FLAG = 0x04 _FAILSAFE_FLAG = 0x08 def __init__(self): self._payload = WriteBuffer(length=self._BUFFER_LEN) self._frame = SbusFrame() self._searching = True def decode(self, b): if self._searching: if b == self._START_BYTE: self._payload.reset_offset() self._searching = False else: _logger.warning("ignoring 0x%02X", b) elif self._payload.has_remaining(): self._payload.write_u8(b) else: self._searching = True return self._parse() return None def _parse(self): buffer = self._payload.get_buffer() ch = 0 start = 0 shift = 0 def from_bytes(b): int.from_bytes(b, ByteOrder.LITTLE) # Channels are 11 bits, usually they're split over two bytes, e.g. 5 bits in one byte and 6 in the next. # But at points they're split over 3 bytes, e.g. 2 bits, 8 bits and 1 bit. while start < 21: b = buffer[start:start + 3] self._frame.channels[ch] = (from_bytes(b) >> shift) & 0x7FF ch += 1 if shift >= 5: start += 2 shift -= 5 # I.e. `shift += (3 - 8)`. else: start += 1 shift += 3 last = buffer[22] # Channels 16 and 17 are either fully on or fully off. self._frame.channels[ 16] = SbusFrame.LEVEL_MAX if last & self._CH16_FLAG else 0 self._frame.channels[ 17] = SbusFrame.LEVEL_MAX if last & self._CH17_FLAG else 0 self._frame.lost_frame = last & self._LOST_FRAME_FLAG != 0 self._frame.failsafe = last & self._FAILSAFE_FLAG != 0 return self._frame
def __init__(self): self._frame = Frame() self._encoded = WriteBuffer(length=self._BUFFER_LEN)
def __init__(self): self._frame = Frame() self._decoded = WriteBuffer(buffer=self._frame.buffer) self._checksum_total = 0 self._escaping = False
class MspRequestDecoder: _BUFFER_LEN = 64 # TODO: how long are the actual incoming requests? _VERSION = 1 _VER_SHIFT = ffs(MspHeaderBits.VERSION_MASK) def __init__(self): self._frame_payload = ReadBuffer() self._request = WriteBuffer(length=self._BUFFER_LEN) self._result = MspRequestResult() self._command = -1 self._started = False self._last_seq = -1 # All frames start with a header byte. # The header is 8 bits - vvvsnnnn - 3 version bit, 1 start bit and 4 sequence number bits. # * If the start bit is set then the header is followed by: # * A payload length byte. # * A command byte, i.e. a byte indicating the purpose of the complete message. # The remainder of the frame is payload bytes (if payload length is greater than 0). # * If the start bit is not set then the rest of the frame is further payload data. # The very last byte (after payload length bytes have been accumulated) is a checksum byte. # If the message is very short then everything including the checksum may fit in a single frame. # A minimal message would be e.g. 0x30 0x00 0x01 0x01 # header^ len^ cmd^ ^checksum # If len were greater than 0 then there would be payload bytes between cmd and checksum. def decode(self, request_payload): self._frame_payload.set_buffer(request_payload) header = self._frame_payload.read_u8() version = (header & MspHeaderBits.VERSION_MASK) >> self._VER_SHIFT if version != self._VERSION: return self._result.set(error=MspError.VERSION_MISMATCH) seq_number = header & MspHeaderBits.SEQUENCE_MASK is_start = header & MspHeaderBits.START_FLAG if is_start: self._request.reset_offset() self._request.set_length(self._frame_payload.read_u8()) self._command = self._frame_payload.read_u8() self._started = True elif not self._started: _logger.warning("ignoring frame %s", request_payload) return None elif seq_number != (self._last_seq + 1) & MspHeaderBits.SEQUENCE_MASK: _logger.error("packet loss between %d and %d", self._last_seq, seq_number) self._started = False return None self._last_seq = seq_number frame_remaining = self._frame_payload.remaining() request_remaining = self._request.remaining() remaining = min(frame_remaining, request_remaining) self._request.write(self._frame_payload.read(remaining)) # Either the frame was totally consumed (and we need more of them) or it has the final checksum byte. if not self._frame_payload.has_remaining(): return None self._started = False request_payload = self._request.get_buffer() checksum = len(request_payload) ^ self._command for b in request_payload: checksum ^= b # Compare the expected checksum with the actual checksum. if checksum != self._frame_payload.read_u8(): return self._result.set(self._command, error=MspError.CHECKSUM) return self._result.set(self._command, payload=request_payload)