def test_calculate_crc16(self):
test_data = {0xA0: 0xFEC2, 0xA1: 0x7EC7, 0x5A: 0xFCDE, 0x35: 0xFDBC}
for data, crc in test_data.items():
if not isinstance(data, (tuple, list, set)):
data = [data]
self.assertEqual(crc, calculate_crc16(data))
self.assertEqual(
0xCD9F,
calculate_crc16([
0x35, 0x85, 0x0C, 0x00, 0xCE, 0xFD, 0xCF, 0xF2, 0x00, 0x00,
0x80
]))
def send_stickcommand():
array_bytes = create_packet(stickCommand, 0x60, 11)
instructions = (0).to_bytes(2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
axis1 = int(660.0 * rx + 1024.0)
axis2 = int(660.0 * ry + 1024.0)
axis3 = int(660.0 * ly + 1024.0)
axis4 = int(660.0 * lx + 1024.0)
axis5 = int(throttle)
packed = (axis1) & 0x7FF | (axis2 & 0x7FF) << 11 | (
0x7FF & axis3) << 22 | (0x7FF & axis4) << 33 | (axis5) << 44
array_bytes.append((0xFF & packed).to_bytes(1, byteorder='little'))
array_bytes.append((packed >> 8 & 0xFF).to_bytes(1, byteorder='little'))
array_bytes.append((packed >> 16 & 0xFF).to_bytes(1, byteorder='little'))
array_bytes.append((packed >> 24 & 0xFF).to_bytes(1, byteorder='little'))
array_bytes.append((packed >> 32 & 0xFF).to_bytes(1, byteorder='little'))
array_bytes.append((packed >> 40 & 0xFF).to_bytes(1, byteorder='little'))
now = datetime.datetime.now()
array_bytes.append((now.hour).to_bytes(1, byteorder='little'))
array_bytes.append((now.minute).to_bytes(1, byteorder='little'))
array_bytes.append((now.second).to_bytes(1, byteorder='little'))
array_bytes.append(
(int(time.time() * 10) & 0xff).to_bytes(1, byteorder='little'))
array_bytes.append((int(time.time() * 10) >> 8).to_bytes(
4, byteorder='little')[0].to_bytes(1, byteorder='little'))
instructions = (crc16.calculate_crc16(array_bytes)).to_bytes(
2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
return b''.join(array_bytes)
def mock_receive(self, _: int) -> Tuple[bool, List[int]]:
data = None
if self.__received_data:
if self.__received_command == 0x03:
data = [0x03, 0x80, self.__received_data[2]
] + [0x00] * self.__received_data[2] * 4
crc = calculate_crc16(data)
data += [crc & 0xFF, crc >> 8]
else:
data = self.__valid_responses[self.__received_command]
return True, data
def send(self, command: int, data: List[int] = None) -> bool:
"""
Sends a command with the given data to the Carmen sensor.
The CRC16 is calculated and appended.
:param command: Command to send.
:param data: Data to send if necessary. Default ist None.
:return: True on success, else false.
"""
if data is None:
data = []
crc = calculate_crc16([command] + data)
return self._send_raw([command] + data + [crc & 0xFF, crc >> 8])
def start_video():
array_bytes = create_packet(videoStartCommand, 0x60, 0)
instructions = (0).to_bytes(2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
instructions = (crc16.calculate_crc16(array_bytes)).to_bytes(
2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
return b''.join(array_bytes)
def land():
global seq
array_bytes = create_packet(landCommand, 0x68, 1)
seq += 1
instructions = seq.to_bytes(2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
instructions = (crc16.calculate_crc16(array_bytes)).to_bytes(
2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
return b''.join(array_bytes)
def flip(direction):
global seq
array_bytes = create_packet(flipCommand, 0x70, 1)
seq += 1
instructions = seq.to_bytes(2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
array_bytes.append(direction.to_bytes(1, byteorder='little'))
instructions = (crc16.calculate_crc16(array_bytes)).to_bytes(
2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
return b''.join(array_bytes)
def set_videoencoder_rate(rate):
global seq
array_bytes = create_packet(videoEncoderRateCommand, 0x68, 1)
seq += 1
instructions = seq.to_bytes(2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
array_bytes.append(rate.to_bytes(1, byteorder='little'))
instructions = (crc16.calculate_crc16(array_bytes)).to_bytes(
2, byteorder='little')
array_bytes += [
instructions[0].to_bytes(1, byteorder='little'),
instructions[1].to_bytes(1, byteorder='little')
]
return b''.join(array_bytes)
def receive(self, size: int) -> Tuple[bool, List[int]]:
"""
Receives data from the Carmen sensor.
The CRC16 is checked.
:param size:
:return: True on success, else false.
:return: The received data.
"""
success, data = self._receive_raw(size)
if success:
# check crc
success = calculate_crc16(data[:-2]) == (data[-2] +
(data[-1] << 8))
if not success:
logging.error('invalid crc')
return success, data
def side_effect(size):
d = [0] * (size - 2)
crc = calculate_crc16(d)
return d + [crc & 0xFF, crc >> 8]