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)