class DiffReader(UtilsDiffReader):
_CF_BW = bitstruct.compile('u5u1u4u6u2u1u1u1u11')
_CF_BL = bitstruct.compile('u5u1u10u2u1u1u1u11')
def __init__(self, ffrom, to_size, bw, bl, ldr, ldr_w, data_pointers,
code_pointers, bw_blocks, bl_blocks, ldr_blocks, ldr_w_blocks,
data_pointers_blocks, code_pointers_blocks):
super().__init__(ffrom, to_size)
self._write_s32_values_to_to(ldr_blocks, ldr)
self._write_s32_values_to_to(ldr_w_blocks, ldr_w)
self._write_bl_values_to_to(bl_blocks, bl)
self._write_bw_values_to_to(bw_blocks, bw)
if data_pointers_blocks is not None:
self._write_s32_values_to_to(data_pointers_blocks, data_pointers)
if code_pointers_blocks is not None:
self._write_s32_values_to_to(code_pointers_blocks, code_pointers)
self._fdiff.seek(0)
def _write_bw_values_to_to(self, bw_blocks, bw):
self._write_values_to_to_with_callback(bw_blocks, bw, self._pack_bw)
def _write_bl_values_to_to(self, bl_blocks, bl):
self._write_values_to_to_with_callback(bl_blocks, bl, self._pack_bl)
def _pack_bw(self, value):
if value < 0:
value += (1 << 25)
t = (value & 0x1)
cond = ((value >> 1) & 0xf)
imm32 = (value >> 5)
s = (imm32 >> 19)
j2 = ((imm32 >> 18) & 0x1)
j1 = ((imm32 >> 17) & 0x1)
imm6 = ((imm32 >> 11) & 0x3f)
imm11 = (imm32 & 0x7ff)
value = self._CF_BW.pack(0b11110, s, cond, imm6, 0b10, j1, t, j2,
imm11)
return bitstruct.byteswap('22', value)
def _pack_bl(self, imm32):
if imm32 < 0:
imm32 += (1 << 24)
s = (imm32 >> 23)
i1 = ((imm32 >> 22) & 0x1)
i2 = ((imm32 >> 21) & 0x1)
j1 = -((i1 ^ s) - 1)
j2 = -((i2 ^ s) - 1)
imm10 = ((imm32 >> 11) & 0x3ff)
imm11 = (imm32 & 0x7ff)
value = self._CF_BL.pack(0b11110, s, imm10, 0b11, j1, 0b1, j2, imm11)
return bitstruct.byteswap('22', value)
def __initializeDataFormat(self):
try:
__bpr = bitstruct.compile('u10u2s32u28s16')
__aclo = bitstruct.compile('u10u2s32s20s20s20')
self.__bpr = __bpr
self.__aclo = __aclo
except:
print("failed to initialize data format...")
win32evtlogutil.ReportEvent(
"MQQT_CLIENT", 32011, eventCategory=32011,
eventType=win32evtlog.EVENTLOG_ERROR_TYPE, strings=["failed to initialize data format..."],
data=b"failed to initialize data format...")
class DiffReader(UtilsDiffReader):
_CF_ADD = bitstruct.compile('u8u2u12u5u5')
_CF_ADRP = bitstruct.compile('u1u2u5u19u5')
def __init__(self, ffrom, to_size, b, bl, add, add_generic, ldr, adrp,
str_, str_imm_64, data_pointers, code_pointers, b_blocks,
bl_blocks, add_blocks, add_generic_blocks, ldr_blocks,
adrp_blocks, str_blocks, str_imm_64_blocks,
data_pointers_blocks, code_pointers_blocks):
super().__init__(ffrom, to_size)
self._write_s32_values_to_to(b_blocks, b)
self._write_s32_values_to_to(bl_blocks, bl)
self._write_add_values_to_to(add_blocks, add)
self._write_s32_values_to_to(add_generic_blocks, add_generic)
self._write_s32_values_to_to(ldr_blocks, ldr)
self._write_adrp_values_to_to(adrp_blocks, adrp)
self._write_s32_values_to_to(str_blocks, str_)
self._write_s32_values_to_to(str_imm_64_blocks, str_imm_64)
if data_pointers_blocks is not None:
self._write_u64_values_to_to(data_pointers_blocks, data_pointers)
if code_pointers_blocks is not None:
self._write_u64_values_to_to(code_pointers_blocks, code_pointers)
self._fdiff.seek(0)
def _write_add_values_to_to(self, blocks, from_dict):
self._write_values_to_to_with_callback(blocks, from_dict,
self._pack_add)
def _write_adrp_values_to_to(self, blocks, from_dict):
self._write_values_to_to_with_callback(blocks, from_dict,
self._pack_adrp)
def _pack_add(self, value):
imm12 = (value & 0xfff)
shift = ((value >> 12) & 0x3)
r = ((value >> 14) & 0x1f)
value = self._CF_ADD.pack(0b10010001, shift, imm12, r, r)
return value[::-1]
def _pack_adrp(self, value):
immlo = (value & 0x3)
immhi = ((value >> 2) & 0x7ffff)
rd = ((value >> 21) & 0x1f)
value = self._CF_ADRP.pack(0b1, immlo, 0b10000, immhi, rd)
return value[::-1]
def test_compile(self):
cf = bitstruct.compile('u1u1s6u7u9')
packed = cf.pack(0, 0, -2, 65, 22)
self.assertEqual(packed, b'\x3e\x82\x16')
unpacked = cf.unpack(b'\x3e\x82\x16')
self.assertEqual(unpacked, (0, 0, -2, 65, 22))
def test_compile(self):
cf = bitstruct.compile('u1u1s6u7u9')
packed = cf.pack(0, 0, -2, 65, 22)
self.assertEqual(packed, b'\x3e\x82\x16')
unpacked = cf.unpack(b'\x3e\x82\x16')
self.assertEqual(unpacked, (0, 0, -2, 65, 22))
def test_bad_format(self):
"""Test of bad format.
"""
formats = [('g1', "bad char 'g' in format"),
('s1u1f32b1t8r8G13', "bad char 'G' in format"),
('s1u1f32b1t8r8G13S3', "bad char 'G' in format"),
('s', "bad format 's'"), ('1', "bad format '1'"),
('ss1', "bad format 'ss1'"), ('1s', "bad format '1s'"),
('foo', "bad format 'foo'"), ('s>1>', "bad format 's>1>'")]
for fmt, expected_error in formats:
with self.assertRaises(Error) as cm:
bitstruct.compile(fmt)
self.assertEqual(str(cm.exception), expected_error)
def test_compile_pack_unpack_formats(self):
fmts = [
('u1s2p3', None, (1, -1)),
('u1 s2 p3', None, (1, -1)),
('u1s2p3', ['a', 'b'], {'a': 1, 'b': -1})
]
for fmt, names, decoded in fmts:
if names is None:
cf = bitstruct.compile(fmt)
packed_1 = cf.pack(*decoded)
packed_2 = pack(fmt, *decoded)
else:
cf = bitstruct.compile(fmt, names)
packed_1 = cf.pack(decoded)
packed_2 = pack_dict(fmt, names, decoded)
self.assertEqual(packed_1, b'\xe0')
self.assertEqual(packed_2, b'\xe0')
def test_compile_pack_unpack_formats(self):
fmts = [
('u1s2p3', None, (1, -1)),
('u1 s2 p3', None, (1, -1)),
('u1s2p3', ['a', 'b'], {'a': 1, 'b': -1})
]
for fmt, names, decoded in fmts:
if names is None:
cf = bitstruct.compile(fmt)
packed_1 = cf.pack(*decoded)
packed_2 = pack(fmt, *decoded)
else:
cf = bitstruct.compile(fmt, names)
packed_1 = cf.pack(decoded)
packed_2 = pack_dict(fmt, names, decoded)
self.assertEqual(packed_1, b'\xe0')
self.assertEqual(packed_2, b'\xe0')
def bitstruct_for(t):
try:
bs = t._binary_bitstruct
except AttributeError:
word_count = t.size
word_size = t.word_size
fmt = ('u' + str(word_size)) * word_count
bs = bitstruct.compile(fmt)
assert bs.calcsize() == word_size * word_count, \
(bs.calcsize(), word_size, word_count)
t._binary_bitstruct = bs
return bs
def test_byte_order_format(self):
"""Test of a format with only byte order information.
"""
cf = bitstruct.compile('>')
self.assertEqual(cf.pack(), b'')
self.assertEqual(cf.pack(1), b'')
self.assertEqual(cf.unpack(b''), ())
self.assertEqual(cf.unpack(b'\x00'), ())
def test_empty_format(self):
"""Test of empty format type.
"""
cf = bitstruct.compile('')
self.assertEqual(cf.pack(), b'')
self.assertEqual(cf.pack(1), b'')
self.assertEqual(cf.unpack(b''), ())
self.assertEqual(cf.unpack(b'\x00'), ())
def test_empty_format(self):
"""Test of empty format type.
"""
cf = bitstruct.compile('')
self.assertEqual(cf.pack(), b'')
self.assertEqual(cf.pack(1), b'')
self.assertEqual(cf.unpack(b''), ())
self.assertEqual(cf.unpack(b'\x00'), ())
def test_byte_order_format(self):
"""Test of a format with only byte order information.
"""
cf = bitstruct.compile('>')
self.assertEqual(cf.pack(), b'')
self.assertEqual(cf.pack(1), b'')
self.assertEqual(cf.unpack(b''), ())
self.assertEqual(cf.unpack(b'\x00'), ())
def __init__(self, frame_id, frame_len, name, signals, publisher):
self.id = frame_id
self.len = frame_len
self.name = name
self.signals = signals
bitsting = ""
for s in self.signals:
s['value'] = s['default_val']
bitsting += "u" + str(s['size_bits'])
self.publisher = publisher
self.bitsting = bitsting
self.__cf = bitstruct.compile(bitsting)
self.__last_values = {}
def test_bad_format(self):
"""Test of bad format.
"""
formats = [
('g1', "bad char 'g' in format"),
('s1u1f32b1t8r8G13', "bad char 'G' in format"),
('s1u1f32b1t8r8G13S3', "bad char 'G' in format"),
('s', "bad format 's'"),
('1', "bad format '1'"),
('ss1', "bad format 'ss1'"),
('1s', "bad format '1s'"),
('foo', "bad format 'foo'"),
('s>1>', "bad format 's>1>'"),
('s0', "bad format 's0'")
]
for fmt, expected_error in formats:
with self.assertRaises(Error) as cm:
bitstruct.compile(fmt)
self.assertEqual(str(cm.exception), expected_error)
class FportMessageControl(FportMessage):
format = bitstruct.compile(
'u11u11u11u11u11u11u11u11u11u11u11u11u11u11u11u11u4u1u1u1u1<')
def __init__(self, message):
# S.Bus like structure:
# 16 * 11bit channels:
# 0 [ ch1.7 ch1.6 ch1.5 ch1.4 ch1.3 ch1.2 ch1.1 ch1.0]
# 1 [ ch2.4 ch2.3 ch2.2 ch2.1 ch2.0 ch1.10 ch1.9 ch1.8]
# 2 [ ch3.1 ch2.0 ch2.10 ch2.9 ch2.8 ch2.7 ch2.6 ch2.5]
# ...
# 21 [ ch16.10 ch16.9 ch16.8 ch16.7 ch16.6 ch16.5 ch16.4 ch16.3]
# 22 flag byte [0 0 0 0 failsafe frame_lost ch18 ch17]
print(message.frame)
data = t.format.unpack(message.frame)
self.axis = data[0:16]
self.switches = data[16:18]
self.frame_lost = data[18]
self.failsafe = data[19]
pass
def __str__(self):
return "Control - ax:{}, sw:{}, frame Lost/FS: {}/{}".format(
self.axis, self.switches, self.frame_lost, self.failsafe)
def __init__(self, *args):
super(ts_ts_1_2_3, self).__init__(*args)
fields = {
'pulseId': 'u64',
'timestamp': 'u64',
'fixed_rate_markers': 'u10',
'ac_rate_markers': 'u6',
'ac_time_slot': 'u3',
'ac_time_slot_phase': 'u13',
'ebeam_present': 'b1',
'reserved1': 'u3',
'ebeam_destination': 'u4',
'reserved2': 'u8',
'requested_ebeam_charge_pc': 'u16',
'requested_ebeam_energy_loc1': 'u16',
'requested_ebeam_energy_loc2': 'u16',
'requested_ebeam_energy_loc3': 'u16',
'requested_ebeam_energy_loc4': 'u16',
'requested_photon_wavelength_sxu': 'u16',
'requested_photon_wavelength_hxu': 'u16',
'reserved3': 'u16',
'mps_limit': 'u16', # one bit per destination
'mps_power_class': 'u64', # four bits per destination
}
self.TsData = namedtuple('tsdata', fields.keys())
format_string = ''
total_len = 0
for v in fields.values():
format_string += v
total_len += int(v[1:])
format_string += '<' # indicated least-significant-byte first
self.total_bytes = total_len // 8
self.bitstructure = bitstruct.compile(format_string)
def test_compile_formats(self):
bitstruct.compile('p1u1')
bitstruct.compile('p1u1', ['a'])
def __init__(self, name:str, d: OrderedDict):
self.d = d
self.fstr = self._parse_format_str( d )
self.compiled_fstr = bs.compile(self.fstr)
self.name = name
self.names = namedtuple( name, d.keys() )
import struct
import bitstruct
SBUS_INPUT_MIN=192
SBUS_INPUT_MID=992
SBUS_INPUT_MAX=1792
SBUS_INPUT_RANGE = ((SBUS_INPUT_MAX - SBUS_INPUT_MIN) - 1)
BAUD = 115200
MARKER = 0x7E
MASK = 0x20
FRAME_SIZE = 0x19
FRAME_TYPE_CONTROL = 0x00
headerStruct = bitstruct.compile('u8u8u8')
channelsStruct = bitstruct.compile('u11u11u11u11u11u11u11u11u11u11u11u11u11u11u11u11u8u8')
footerStruct = bitstruct.compile('u8u8')
def crc(data):
return 0x00
def cleanData(data):
out = []
for d in data:
out.append(d)
if( d == MARKER or d == 0x7D ):
out.append(d ^ MASK)
return out
def writeControls(channels, rssi, flags):
from xrpc.serde.types import ISO8601
from xrpc.util import time_now
RPC_TS_FORMAT_SPEC = [
('year', 12, 12, 1.5, 4096),
('month', 4, 16, 2, 16),
('day', 5, 21, 2.625, 32),
('hour', 5, 26, 3.25, 32),
('minute', 6, 32, 4, 64),
('second', 6, 38, 4.75, 64),
('microsecond', 20, 58, 7.25, 1048576),
('_reserved', 6, 64, 8, 64),
]
RPC_TS_FMT = ''.join(f'u{x}' for _, x, *_ in RPC_TS_FORMAT_SPEC)
RPC_TS_FMT_COMPILED = bitstruct.compile(RPC_TS_FMT)
RPC_TS_COMPILED_SIZE = RPC_TS_FMT_COMPILED.calcsize() // 8
RPC_UUID_SIZE = 16
RPC_KEY_SIZE = RPC_TS_COMPILED_SIZE + RPC_UUID_SIZE
def time_pack(dt: datetime):
timestamp = RPC_TS_FMT_COMPILED.pack(
*(getattr(dt, x) if not x.startswith('_') else 0
for x, *_ in RPC_TS_FORMAT_SPEC))
return timestamp
def time_unpack(body: bytes) -> datetime:
to_unpack = zip((x for x, *_ in RPC_TS_FORMAT_SPEC),
inputFile = open(sys.argv[1], "rb")
else:
ser = serial.Serial(port='/dev/ttyUSB0', baudrate=baud, stopbits=1)
ser.flushInput()
size = 1024
pause_time = 0.1
msgGyro = struct.Struct('< i i i')
msgRx = struct.Struct('< H H H H H B B')
msgMotor = struct.Struct('< i i i i')
msgInput = struct.Struct('< i i i i i')
msgSetpoint = struct.Struct('< i i i i i')
msgStatus = bitstruct.compile('u8u8u8u8u8u8u8')
x = np.arange(size)
gyroData = [[0] * size, [0] * size, [0] * size]
gyroFFtData = [[0] * size, [0] * size, [0] * size]
inputsData = [[0] * size, [0] * size, [0] * size, [0] * size]
setpointData = [[0] * size, [0] * size, [0] * size, [0] * size]
motorData = [[0] * size, [0] * size, [0] * size, [0] * size]
rxData = [[0] * size, [0] * size, [0] * size, [0] * size]
inputData = []
readByteData = bytearray()
serialBufferData = [0] * size
import struct
import bitstruct
from io import BytesIO
from construct import Container
from xbox.nano import enum, packer
from xbox.nano.enum import (RtpPayloadType, ChannelControlPayloadType,
ChannelClass, VideoPayloadType, AudioPayloadType,
InputPayloadType, ControlPayloadType,
ControllerEvent)
pack = packer.pack
RTP_FLAGS = bitstruct.compile('u2b1b1u4b1u7')
VIDEO_CONTROL_FLAGS = bitstruct.compile('p2b1b1b1b1b1b1p24')
AUDIO_CONTROL_FLAGS = bitstruct.compile('p1b1p1b1b1p27')
STREAMER_TYPE_MAP = {
ChannelClass.Video: VideoPayloadType,
ChannelClass.Audio: AudioPayloadType,
ChannelClass.ChatAudio: AudioPayloadType,
ChannelClass.Input: InputPayloadType,
ChannelClass.InputFeedback: InputPayloadType,
ChannelClass.Control: lambda _: 0
}
class PackerError(Exception):
pass
def test_compile_formats(self):
bitstruct.compile('p1u1')
bitstruct.compile('p1u1', ['a'])
import bitstruct
import socket
from .frame import Frame
from .utils import *
# initiate globals
l2tp_header_formatter = bitstruct.compile('b1b1p2b1p1b1b1p4u4u16u16u16u16u16')
avp_header_formatter = bitstruct.compile('b1b1p4u10u16u16')
avp_attribute_types = [(0, 'control_message', 'u16'),
(2, 'protocol_version', 'u8u8'),
(3, 'framing_capabilities', 'p30b1b1'),
(4, 'bearer_capabilities', 'p30b1b1'),
(6, 'firmware_revision', 'u16'),
(7, 'host_name', 't{len}'),
(8, 'vendor_name', 't{len}'),
(9, 'assigned_tunnel_id', 'u16'),
(10, 'receive_window_size', 'u16'),
] # attribute_type, name, unpack string
control_message_types = [(1, 'SCCRQ'), # Start-Control-Connection-Request
(2, 'SCCRP'), # Start-Control-Connection-Reply
(3, 'SCCCN'), # Start-Control-Connection-Connected
(4, 'StopCNN'), # Stop-Control-Connection-Notification
(6, 'HELLO'), # Hello
(7, 'OCRQ'), # Outgoing-Call-Request
(8, 'OCRP'), # Outgoing-Call-Reply
(9, 'OCCN'), # Outgoing-Call-Connected
(10, 'ICRQ'), # Incoming-Call-Request
(11, 'ICRO'), # Incoming-Call-Reply
(12, 'ICCN'), # Incoming-Call-Connected
