72 lines
2.3 KiB
Python
72 lines
2.3 KiB
Python
from enum import IntEnum
|
|
import struct
|
|
from pyblake2 import blake2s
|
|
import time
|
|
import toml
|
|
|
|
HASH_LENGTH = 8
|
|
|
|
with open("Config.toml", "r") as config_file:
|
|
config = toml.loads(config_file.read())
|
|
|
|
print(config)
|
|
devices = {}
|
|
|
|
|
|
class MessageType(IntEnum):
|
|
DeviceStatus = 1
|
|
SensorStatus = 2
|
|
|
|
|
|
def decode_packet(data):
|
|
packet_type = data[0]
|
|
|
|
# match packet_type:
|
|
# case MessageType.DeviceStatus:
|
|
|
|
if packet_type == MessageType.DeviceStatus:
|
|
return {"Battery voltage": struct.unpack('<f', data[1:5])[0]}
|
|
|
|
if packet_type == MessageType.SensorStatus:
|
|
channels_raw = struct.unpack('<H', data[1:3])[0]
|
|
channels = []
|
|
for i in range(16):
|
|
if (channels_raw >> i) & 1:
|
|
channels.append(i)
|
|
|
|
sensor_data = []
|
|
for i in range(len(channels)):
|
|
offset = i * 6
|
|
sensor_data.append({
|
|
"channel": channels[i],
|
|
"type": data[3 + offset],
|
|
"pin": data[4 + offset],
|
|
"value": struct.unpack('<f', data[5 + offset : 9 + offset])[0]
|
|
})
|
|
return sensor_data
|
|
|
|
|
|
def process_packet(payload):
|
|
rx_id = int.from_bytes(payload.message[0:2], byteorder="little")
|
|
tx_id = int.from_bytes(payload.message[2:4], byteorder="little")
|
|
msg_id = int.from_bytes(payload.message[4:8], byteorder="little")
|
|
length = payload.message[8]
|
|
data = payload.message[9: 9 + length]
|
|
data_hash = payload.message[9 + length: 9 + length + HASH_LENGTH]
|
|
|
|
if len(payload.message) != length + 9 + HASH_LENGTH:
|
|
print(
|
|
f"Invalid length! Expected {length + 9 + HASH_LENGTH} actual {len(payload.message)}")
|
|
return
|
|
|
|
hash_function = blake2s(key=0x0.to_bytes(8, "little"), digest_size=8)
|
|
hash_function.update(payload.message[: -HASH_LENGTH])
|
|
|
|
if hash_function.digest() != data_hash:
|
|
print(
|
|
f"Hash doesn't match! Expected {hash_function.digest()} got {data_hash}")
|
|
return
|
|
|
|
# print(f"{tx_id} #{msg_id}: {decode_packet(data):.3f} V, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
|
|
print(f"{tx_id} #{msg_id}: {data.hex()} {decode_packet(data)}, {payload.rssi} dB(?) RSSI, {payload.snr} dB(?) SNR {(time.clock_gettime_ns(0)) / 1e9}")
|